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ABSTRACT 



The ability to conduct research in the robotic field in new areas can be accomplished safely and 
efficiently using computer graphic simulation. Object-oriented languages provide a powerful and 
flexible capability in defining rigid body manipulators that can be adapted in the use and design of 
many types of systems. The very nature of object-oriented programming permits modification and 
improvement of the code with ease. 

This thesis examines the major capabilities of object-oriented programming in conjunction with 
kinematics equations that simulate a six-legged walking robot. A comparison is conducted between 
programs using CLOS (LISP) and C-t-+ to graphically simulate the Aquarobot - an existing 
underwater walking robot. It is found that both languages are effective, but CLOS programming is 
easier while C+ -I- code executes more than twice as fast as compiled CLOS. 
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I. INTRODUCTION 



A. GOALS 

The goal of this thesis is to investigate a method for 
producing a graphic simulation of a walking robot constructed 
from serial manipulators acting as legs. The main intent is 
to compare object-oriented code that is based on kinematics 
using two programming languages, CLOS and C++. This thesis 
discusses and provides examples of steps necessary for the 
evolution of a first stage graphic simulator of a walking 
robot. The walking robot in question is a six-legged 
underwater vehicle, called "Aquarobot” , that is presently 
under development in Japan for use in subsea construction and 
inspection tasks. 

B. ORGANIZATION 

Chapter II of this thesis reviews previous work in the 
area of walking robots. Chapter III provides a detailed 
description of Aquarobot, the subject of the simulator 
developed in this research. Chapter IV provides an overview 
of kinematics modelling of articulated rigid bodies, and 
methods used to calculate link parameters for such systems. 
The last part of this chapter provides the specific kinematic 
parameters for Aquarobot. 
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Chapter V is a review of object-oriented programming and 
includes a discussion of its advantages and disadvantages. 
Chapter VI contains the history and a description of some 
common object-oriented languages. Chapter VII provides a 
description of the Aquarobot simulation programs written in 
the CLOS and C++ languages. This chapter compares the methods 
each language requires to define classes and create objects. 
A comparison of the performance of the C++ and CLOS 
simulations is provided in Chapter VIII. 

The last chapter, Chapter IX, presents some conclusions 
about the work described. This is followed by recommendations 
for possible future use of Aquarobot, the characteristics of 
the two simulations created, and suggestions for further 
research. 
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II. SURVEY OF PREVIOUS WORK 



A. INTRODUCTION 

Man's need to comprehend the human body and the phenomena 
around him motivates him to imitate it as a tool of 
understanding. This chapter provides a historical review of 
robotic advancements in living animal imitation. It 
specifically addresses the evolution of legged robots. The 
differences between legged and wheeled locomotion are also 
discussed. 

B. HISTORICAL IMITATION OF LIVING CREATURES 

Historically, research has attempted to build machines 
that imitate animals. Through technology, it is hoped to 
achieve a better understanding of humans and animals and to 
accomplish these creature's tasks with robots. Some such 
research is driven by a desire to provide the disabled with 
alternative compensation methods, such as artificial limbs, 
and other means of achieving increased mobility (McGhee, 
1977) . Mobility goals for legged vehicles include moving 
faster or for extended times, or operating in adverse 
environments and conditions such as moving under water, and in 
space flight applications. 

Biological systems, often taken for granted, are extremely 
difficult to emulate or even define. One example, the 



3 



imitation of a walking gait of an animal, is not easy due to 
the difficulty of emulating the nervous system and the natural 
materials that form the animal. These unknown variables have 
impeded our success in obtaining the coordination algorithms 
of even simple animals (McGhee, 1985) . A human takes 
approximately one year to learn how to walk yet, after decades 
of research, walking machines are still considered to be in 
the "infant" stage. 

Animal limb imitation has been an area of great interest 
to researchers interested in advanced mobility systems. If an 
application for a walking vehicle is known, there are many 
variables that must be considered to determine an animal to 
imitate. As an example, one variable is compliance (Anon, 
1987) . Compliance is defined as "the act of conforming, 
acquiescing, or yielding" (Stein, 1979) . As the degree of 
compliance of a design is improved, the machine becomes more 
challenging to control and keep the limb steady, yet it will 
be more robust (e.g., able to withstand impact). If the 
degree of compliance in a design is reduced, then the ability 
to accurately position the limb will be enhanced, but it will 
tend to be rigid and unyielding. On the other hand, 
compliance permits flexibility which is beneficial when 
performing simple but complex actions such as attempting to 
place a bolt on a screw (Anon, 1987) . 

Limb imitation designs have varied drastically in 
appearance. For access to tight spaces, snake-like devices 
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have been constructed. Their applications require that 
compliance be limited in order to maintain position. In 
contrast, a limb similar to an elephant's trunk has been used 
as a device to lift objects of varied shapes. This device did 
not have an internal support structure. Instead, it copied the 
multiple layers of muscle in an elephant's trunk which 
provides motion control. It was extremely compliant in order 
to accommodate the varied shapes that grasped objects require 
(Anon, 1987) . Human hands have been imitated in numerous 
designs. Additionally, legs are very popular in robot 
research. 

Legged locomotion requires a successful leg design. 
Through evolution, animals have perfected their individual 
legged locomotion characteristics based on their specialized 
needs. Legged animals are capable of high speeds and 
intricate motion even when the animal is large and heavy. 
Animal legs have been put into two categories: "mammal” and 
"insect” types (Iwasaki, 1987) . The "mammal” type has legs 
which are always vertical like a horse. The "insect” type has 
bent legs like a beetle. A walking capability able to 
function in natural terrain requires complicated sensors, a 
nervous system, and artificial intelligence (e.g., a reasoning 
ability) . Since exact imitation of these intricate animal 
systems has not, at present, been achieved, legged vehicle 
designers must choose other means to solve this coordination 
control problem (McGhee, 1985) . 
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C. HISTORY OF WALKING ROBOTS 



The original legged machines evolved from earth moving and 
construction vehicles. These devices are known as "climbing 
hoes” (McGhee, 1985). From 1965 to 1968, a four-legged 
vehicle, called the "Quadruped Transporter", was constructed 
by General Electric. This vehicle incorporated a human 
operator in order to provide the sensing and neural control 
functions discussed earlier. The operator of this vehicle was 
provided with one leg control lever for each limb. These 
levers were attached to the arms and legs of the human 
operator so that he could control the legs by executing the 
desired motions with his own limbs. The front legs were 
controlled by the operator's hands and the rear legs were 
controlled by the operator's feet. Each control lever had 
three degrees of freedom: two at the hip and one at the knee. 
This coordination control system required a high level of 
operator skill, and only a few mastered its intricacies. 
Moreover, these operators could only walk the vehicle for a 
short time due to the complicated multi-degree of freedom 
coordination problem (McGhee, 1985) . 

The Quadruped Transporter was designed as a research 
vehicle and opened the field of vehicular legged locomotion. 
A hydraulic servo system moved the legs. It successfully 
walked and displayed impressive obstacle climbing ability. 
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However, the complexity of the operator motion coordination 
task severely limited the device's capabilities (McGhee, 
1985) . 

In 1977, a different control method was incorporated into 
another robot called the Ohio State University (OSU) Hexapod 
Vehicle. This robot used supervisory control (Ferrell, 1967) 
in which the operator controlled speed and direction, and a 
computer coordinated the actual leg motion (Pugh, 1982) . The 
OSU Hexapod Vehicle was a six-legged vehicle with insect type 
legs (McGhee, 1985) . The device was constructed to study and 
develop gait algorithms. Each leg had three degrees of 
freedom, each consisting of two links connected by a joint. 
Each joint had an electric motor and a worm gear (Waldron, 
1989) . The operator controlled the vehicle with a remote 
joystick in an indoor laboratory setting. 

The successor to the OSU Hexapod Vehicle was completed in 
1986 at OSU. It was called the "Adaptive Suspension Vehicle" 
(ASV) (Waldron, 1986) . The ASV was designed for sustained 
outdoor locomotion on uneven and unmapped terrain. This six- 
legged robot was the first robot to control its legs by an on- 
board computer and to carry its own power source in the form 
of an internal combustion engine (Waldron, 1986) . The ASV, 
like the Quadruped Transformer, includes an onboard human 
operator. However, the ASV does not require manual 
coordination of limb motion by the operator (Waldron, 1986) . 
In order for the ASV to operate in unstructured terrain, it 
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incorporates extensive sensor devices including a laser 
terrain scanner to provide a three dimensional terrain 
elevation map for a distance of ten meters in front of the 
vehicle. This information is used for automatic selection of 
footholds in rough terrain (Waldron, 1986) . 

To date, legged vehicles have had limited application 
success. This is due to the complex leg coordination control 
problem and a limited understanding of necessary gait 
algorithms. Also, this situation exists because of limited 
advances in leg design. Future improvements in agility and 
speed are anticipated with further progress in understanding 
of the difficult problem of microcomputer coordination of 
joint motion (McGhee, 1985) . 

D. ADVANTAGES OF LEGGED ROBOTS 

Legged locomotion has existed for hundreds of millions of 
years while wheeled locomotion, an invention of man, has been 
around for only several thousand years (Waldron, 198 9) . It is 
interesting that evolution has not produced wheeled biological 
systems, but then there were no smooth, graded roads before 
the introduction of wheels. Still, given the elegant results 
of evolution, one might conclude legged locomotion is 
inherently superior to wheeled locomotion, at least in natural 
terrain. 

Currently, it is possible to go close to most places of 
interest on the land surface of the earth by traveling on 
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roads. This has greatly altered our environment. Still, it 
takes an off -road wheeled or tracked vehicle to reach the 
areas in between, and they leave ugly ruts in the soil. If 
the off-road vehicle were to be a legged vehicle, it would 
leave only discrete footprints. Furthermore, over half the 
Earth's land surface (largely, unpopulated areas) is entirely 
inaccessible to wheeled vehicles (Waldron, 1989) but not to 
legged vehicles. Legged vehicles have the potential to walk 
underwater and in surf as well. 

Legged locomotion has an advantage over wheeled locomotion 
when soft ground or slippery surfaces are involved. Wheeled 
vehicles sink into the ground and must roll out of the 
resulting depression by relying on shearing forces resulting 
from friction between wheels and the ground. Legs also sink 
into the ground but can be lifted vertically (Bekker, 1969) - 
a maneuver that doesn't impede locomotion. 

While wheeled vehicles have proven themselves efficient 
for long-distance transportation, the path must be relatively 
smooth and firm. The performance of large mammals shows that 
it is possible for legged locomotion to also be efficient for 
long-distance transportation. However, actively coordinated 
leg motions must be defined by algorithms. These algorithms 
are presently in an early stage of development (Waldron, 
1989) . 

Legged vehicles may eventually be able to compete with 
wheeled locomotion in all respects except possibly speed. 
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However, additional technological advances in theory and 
materials will be needed before such machines can reach their 
full potential. The advances in computers in the late 1980 's 
enabled researchers to provide for the leg coordination 
computations on board a walking vehicle, but researchers are 
moving slowly in their attempts to provide sufficiently 
powerful computation algorithms (McGhee, 1985) . Over adverse 
terrain, legged vehicles have the potential to provide higher 
speed, greater mobility, and less environmental damage. 
Additionally, legged vehicles can provide more comfort for a 
human rider. The rough ride wheeled provided by locomotion 
over rough terrain is detrimental to instruments and cargo on 
board. In contrast, legged vehicles do not vibrate when 
travelling over rough terrain (Waldron, 1989) . Finally, 
several studies have shown that legged vehicles have the 
potential to provide improved fuel economy in comparison with 
wheeled vehicles of comparable size (McGhee, 1986) . 

E. SUMMARY 

This chapter provides a survey of previous work relating 
to and walking machines. It specifically discusses the 
history of legged vehicle technology and provides walking 
machine examples. Legged and wheeled locomotion are compared 
and their specific advantages are discussed. The next chapter 
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discusses a walking robot, Aquarobot, that is currently under 
development in Japan, and which provides the focus of this 
thesis. 
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III. AQUAROBOT 



A. INTRODUCTION 

One of Japan's most important resources is its land. 
Unfortunately, Japanese tidal waves (tsunamis) , constantly 
threaten the Japanese coast and erode productive ground. 
Granite rock mound foundations are currently being laid for a 
tsunami seawall to be installed in Kamaishi Bay in the 
northern part of Honshu. This seawall is designed to 
dissipate the energy of a tsunami prior to its arrival at 
shore. The Port and Harbour Research Institute (PHRI) of the 
Ministry of Transportation in Yokosuka, Japan, wishes to 
develop a general method to accomplish deep water structural 
inspection of seawalls, including the Kamaishi project. This 
method should also provide supervision of construction and 
quality control, and should not involve the use of human 
divers (Akizono, 1989) . 

Unfortunately, there is not an "optimal” device to 
accomplish the task that PHRI requires. PHRI is currently 
using human divers to measure wall and foundation variations. 
This is a difficult process due to the pressurization 
requirements of the human body and the short time that divers 
can be at the bottom (about one hour per day at a depth of 
sixty meters) . Additionally, the deep sea diver occupation is 
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physically taxing and it is difficult to recruit new 
personnel. At this time, most of Japan's deep sea divers are 
in their late 30's or older (Takahashi, 1993). Human divers 
are very capable when at the depth of the wall, but are slow 
and expensive. 

Using a robot is one obvious alternative. There are two 
basic options in the design of such a robot. First, a 
floating Remotely Operated Vehicle (ROV) could be used. 
However, floating vehicles have difficulty maintaining a 
stationary position while keeping a specified heading in 
water. A floating vehicle has a poor ability to accurately 
measure objects since the vehicle is not stable. This would 
make a floating robot a poor choice for the PHRI measurement 
needs. However, a floating vehicle is an excellent choice for 
camera inspection because it can move a TV camera to all 
viewing aspects. Unfortunately, if the sea floor is muddy, a 
floating robot may make the water murky due to turbulence 
induced by its thrusters used for maneuvering (Robison, 1992) . 

Another robot type available is the walking robot. It 
provides stability in a stationary position. It can provide 
the measurements PHRI desires. However, there will be 
limitations on the camera angles dependent upon the degrees of 
freedom of the camera arm and the arm placement. A walking 
robot does not muddy the water because it does not stir up a 
soft sea floor. Of the two general types of walking robots, 
"mammal” and "insect”, the insect type provides better 
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movement on uneven terrain (Waldron, 1989) . Aquarobot is an 
insect type walking robot. 

B. AQUAROBOT HISTORY 

PHRI has designed three robots in an attempt to produce 
the first practical underwater walking robot. These robots 
have been labeled "Aquarobot” by their creator, PHRI (Akizono, 
1989) . They are all six-legged articulated robots. 

The first, an experimental model, was designed in 1985. 
It was not watertight and was designed to run ground tests for 
basic research and as a software debugger. 

The second Aquarobot, the prototype model, was designed 
for underwater sea floor applications. The third Aquarobot 
was designed as a lightweight design of the prototype model. 
It is the second prototype model which has been modeled in 
this thesis (Akizono, 1989) . 

C. DESCRIPTION 

The prototype Aquarobot is a walking ROV designed to 
follow a path determined from navigation beacons using a gait 
algorithm computed by a control station on a barge on the 
surface, and passed to the robot via a tether. It is a six- 
legged articulated "insect type" robot equipped with one arm 
used to move and aim a video camera (Akizono, 1989) . 

The aquarobot consists of a hexagonal body and six legs. 
The body is constructed of anti-corrosive aluminum. Each leg 
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has three rotary joints that provide three degrees of freedom. 
Additionally, each leg has a disc-shaped foot pad that is 
attached to the leg with a freely rotating ball joint. The 
foot pads are not position controlled, but are oriented by a 
combination of gravity, the terrain surface, and hydrodynamic 
effects acting on the ball joint connection. Figure 2.1 
depicts the Aquarobot and its leg structure. 




Figure 3.1 

Photograph of Aquarobot 



Each leg joint of Aquarobot is controlled by the computer 
via a DC motor that drives a reduction gear. The reduction 
gear consists of a harmonic gear and a pair of beveled gears. 
This drive method is known as a semi-direct drive mechanism 
(Akizono, 1989) . These motors and gears are located within 
the legs. 
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Each of the eighteen motors are driven by DC power. There 
is one motor driver per motor, each located on the barge 
controlling Aquarobot. The motor driver sends the motor a 
voltage computed from pulse information it receives. 
Specifically, the motor driver contains a pulse counter which 
counts up for pulses received from the computer and counts 
down when pulses are received from encoded motor output 
feedback. The motor driver provides the necessary voltage to 
the motor to drive the counter toward zero. Thus, the 
motor /driver system uses a simple position feedback method. 
(Akizono, 1989) 

There are two inclinometers and one gyrocompass (Anon, 
1992) on the body of Aquarobot. Each foot has a pressure 
sensitive touch sensor. These sensors provide foot contact 
and body orientation information to the computer (Akizono, 
1989) . To measure the absolute elevation of selected points 
on a rock mound foundation, one leg of Aquarobot is also 
furnished with an accurate depth cell located just above the 
foot (Takahashi, 1993) . 

The computer system, located on the barge, provides 
walking algorithms and operating programs. It is a 16-bit 
controller. The interface is provided by two integrated 
circuit boards: an input/output board and an A/D converter 
board. The input/output board sends pulses to the motor 
driver and receives touch sensor status and joint rotation 
pulses from the legs. The A/D converter receives the 
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gyrocompass, depth cell, and inclination sensor feedback. 
Individual leg motions thus are performed using hardware 
controls, while top level motion control and path planning is 
controlled by software. (Akizono, 1989) 

The information bus has changed throughout Aquarobot's 
evolution. The tether for the experimental model consisted of 
copper wire. The two later models have optical fiber links 
with optical/electric converters in the body and control unit. 
However, all models contain eighteen copper wires to carry 
current to individual motors, resulting in a rather large 
cable cross section (four centimeters) . (Iwasaki, 1987) 

The computer software is currently written in BASIC. The 
operating program receives the walking commands from the gait 
algorithm and simultaneously translates them to the motor 
drivers in pulse form. 

The prototype model's video camera arm has three rotary 
joints. Cameras may also have independent pan and tilt 
control. The arm is also equipped with an ultrasonic ranging 
device. Using this device, scales can be projected on the 
camera screen so that measurements of an object can be 
interpreted in conjunction with its range from Aquarobot to 
determine actual dimensions. 

The prototype model also has a relative navigation 
capability which uses a transponder system. This system 
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measures its position in cartesian coordinates, based upon 
triangulation of signals received from beacons placed in the 
vicinity of Aquarobot at known locations. (Akizono, 1989) 

D. CURRENT USE IN JAPAN 

The prototype Aquarobot has successfully walked 
underwater. It's current maximum walking speed on uneven sea 
bed is approximately one meter per minute. While this speed 
is judged to be acceptable, Aquarobot has not been put to 
practical use because human divers are still able to perform 
its function at a lower cost. (Takahashi, 1993) 

E. POSSIBLE AQUAROBOT IMPROVEMENTS 

Aquarobot could be improved in many ways. The physical 
characteristics of the tether and the resultant effects of 
currents on it is an area where substantial improvements are 
possible. The tether could be decreased from its currently 
large circumference and bulky appearance. This could be done 
by improving the motor controllers and placing them in the 
vehicle. In this way, the eighteen wires in the cable 
carrying motor currents could be replaced by a single two 
conductor power cable. Additionally, the computer software 
could be optimized to provide faster and more flexible code. 
New technology in integrated circuits should be incorporated 
to generally decrease component size and power requirements. 
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F. SUMMARY 



Aquarobot represents a major advancement in the field of 
walking robots. Aquarobot 's design was influenced by the 
mission it was to accomplish. This is not often the case in 
robot design. Usually, a robot is designed from a research 
standpoint and then may be used in a "real life" application. 
When an application is driving the technology, robotics 
advancement looks at the problem from a new perspective and 
new and varied designs can be anticipated. The algorithms 
required to calculate the leg and body positions of Aquarobot 
are described in the next chapter of this thesis. 
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IV. KINEMATICS MODEL 



A. INTRODUCTION 

Robots typically consist of one or more ''limbs” which are 
technically defined as mechanical manipulators. These 
manipulators provide the robot with the capability to grasp, 
walk, or perform some other task. To control the robot 
appendages with commands to move to a desired location, 
knowledge from the field of physics and engineering that 
describes motion of rigid bodies is needed. This field is 
known as kinematics . Kinematics is ”... the science of motion 
which treats motion without regard to the forces which cause 
it” (Craig, 1989, p.6). Kinematics allow all geometric 
properties of the motion to be defined. 

Forward kinematics computes the Cartesian space position 
and orientation of the manipulator links from a set of 
parameters which describe the manipulator using angles and 
lengths. The orientation is often described as azimuth, roll, 
and elevation. Inverse kinematics solves for the manipulator 
parameters when the Cartesian space and orientation are known. 

B. LINKAGE AND COMPONENT DESCRIPTION 

Manipulators consist of nearly rigid links which are 
connected at joints. There are two simple types of joints: 
sliding (prismatic) and rotary. The joints are designated by 
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number beginning from the base, usually labeled joint 0 
(Craig, 1989) . The base is also sometimes considered to be the 
most inboard link. The free end of the links is the most 
outboard link and is often called the end-effector . It is at 
the end-effector that the robot's work is performed. Often the 
end-effector is a grasping device or a foot pad. 

Kinematics considers each link to be a purely rigid body 
(Craig, 1989). In reality, description of a manipulator's 
links requires many variables to be considered during the 
design process. Some variables include the material used for 
construction, the link strength, stiffness, length, and the 
manipulator weight. 

Kinematic algorithms are designed to define the position 
and orientation of all manipulators regardless of their 
geometric complexity. This is accomplished by carefully 
defining joint coordinate axes called frames and arranging 
their alignments using standard parameters that describe the 
adjacent link relationships (Craig, 1989) . 

C. KINEMATICS PARAMETER DEFINITIONS 

A frame is attached to each joint with the Z-axis 
coincident with the joint motion axis. The X-axis of the 
frame is directed from a link's inboard joint towards its 
outboard joint to intersect that joint's axis, and is mutually 
perpendicular to both Z-axes. 
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Four parameters are needed in the kinematic algorithms. 
The first, link length , is the distance along the X-axis 
between the joints of a given link. The second is link twist . 
This is the angle necessary to rotate the inboard Z-axis to be 
parallel to the outboard Z-axis. 

The third parameter is link offset . It is the distance 
measured at the inboard link axis from the preceding link X- 
axis to the current X-axis. The final parameter is the 
rotation at this joint from the previous link X-axis to the 
current link X-axis. This is known as the joint angle . 

D. CRAIG VERSUS DANEVIT-HARTENBERG METHOD COMPARISON 

Forward kinematics determines the cumulative effect of 
joint motions on the entire link chain. This cumulative 
effect can be accomplished by a number of methods. Two common 
methods, Craig and Danevit-Hartenberg, are related in their 
approach but differ in their setup (Spong, 1989) . 

To begin with, the manipulator must be inspected. The 
frames must be placed with the proper orientation. The four 
parameters discussed above must then be determined. These 
parameters are identical for both methods; however, the 
numbering of the joint frames varies. 

The Craig method numbers the links beginning with zero at 
the most inboard link. The base joint is numbered joint 0. 
This produces a numbering system where the link and the link's 
inboard joint have the same index number (Craig, 1989) . An 
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example of this notation is pictured in Figure 4.1. The base 
(joint 0) inboard link length and inboard link twist are both 
defined as zero. 



Joint i Joint i+1 




Figure 4 . 1 

Craig Method Frame and Parameter Assignment 
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The Danevit-Hartenberg (DH) notation differs from the 
other method. In this method, the first link, attached to the 
base joint, is labeled link 1. The base joint is labeled 
joint 0. This produces a numbering system along the link 
chain in which the link and the link's outboard joint have the 
same index number (Spong, 1989) . An example of this method is 
pictured in Figure 4.2. 




Figure 4.2 

Danevit-Hartenberg Frame and Parameter Assignment 
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These methods use related conventions for manipulating 
these parameters; however, the algorithms are different. The 
cumulative effect of the links are defined within a matrix 
known as the transformation matrix (Craig, 1989) . The 
transformation matrix differs for the two methods addressed. 

The Craig method uses a transformation matrix (known as 
the T matrix) to define the outboard joint location on a link 
relative to the inboard joint. The T matrix is defined as 
(Craig, 1989, p.84); 
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The subscript of the T matrix label describes which joint is 
being defined. The superscript of the T matrix describes the 
link to which the matrix is referenced. 

The Danevit-Hartenberg method uses a transformation matrix 
(known as the A matrix) to define the location of the inboard 
joint on a link relative to the outboard joint. That is, the 
coordinate origin for a link is located at its outboard joint 
for the DH method, while it is at the inboard joint in the 
Craig method. The A matrix is defined as (Spong, 1989, 

p.66) : 
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The subscript and superscript of the A matrix are defined the 
same as the T matrix above. However, by convention, the index 
is transposed. 

These transformation matrices provide information on the 
rotation and translation needed to superimpose the frame being 
transformed to the relative frame. The rotation information 
is the top left 3x3 sub matrix in the transformation matrix. 
The translation information is in the right column in the 
first three rows. 

The base joint is aligned with the coordinates that the 
designer would like to use to reference the link positions. 
Usually, for fixed base manipulators, the base joint axis is 
aligned with the Earth's coordinates. To transform the joint 
in question, the transformation matrices need to be multiplied 
together (Craig, 1989) . For example: 

^Ot = ,°T * 2*T * 3^T * 4^T (4.5) 
o^A = o'A * ,'A * 2^A * 3^A (4.6) 

E. AQUAROBOT KINEMATICS 

Aquarobot's six legs are identical manipulators except for 
their angle off of the body's forward axis. In order to 
simplify the leg parameters of the first link, an imaginary 
link was constructed from the body's center to the point where 
the leg joins the body. This makes the body's center the base 
joint. The Craig method will be used in this thesis to solve 
the kinematic equations for Aquarobot's legs. 
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1. Aquarobot Leg Parameters 

Common symbols exist for the parameters. They are: 
link length (aj) , link twist (aj , link offset (dj) , and joint 
angle (^j). Figure 4.3 shows one Aquarobot leg with the 
imaginary leg link included. 
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Figure 4 . 3 

Aquarobot Frame Descriptions Of One Leg and The Body 
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Xx 



Figure 4.4 

Top View of Aquarobot Showing First Two Angles 
and Axes for Leg Six 

The parameters for Aquarobot 's legs are shown in Table 
4.1 below. Note that the joint angle of the base (i = 0) is 
the only fixed parameter that varies among the legs. The joint 
angle range for the other joints common to all legs are given. 
These limits are the physical joint ranges. Joint four does 
not have a frame designated because it is a passive ball 
joint. 
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TABLE 4.1 



AQUAROBOT KINEMATICS PARAMETERS 
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2. Transformation Matrices 

The transformation matrices of the Aquarobot legs were 
constructed using Table 4.1 above. The Craig method 
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product of the T matrix multiplication. 

The T matrix product of each joint is called the 
Homogeneous Transformation matrix (i.e., H matrix). To 
determine the next outboard joint's orientation based upon the 
reference frame, the H matrix of the current (relatively 
inboard) joint is multiplied by the outboard joint's T matrix. 
The joint's H matrix provides the orientation from the 
reference frame outboard to that joint. 
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The H matrix for the body provides orientation of the 
body frame (and, in turn, its outboard joints) to the fixed 
reference frame which is usually a designated point on Earth. 
The initial orientation information required is azimuth, 
elevation, roll, and translation from the reference's origin. 
Elevation is defined as rotation of the body X-axis above or 
below the horizontal plane. Azimuth is the rotation of this 
axis away from north about a downward directed axis. Roll is 
rotation about the body X-axis after azimuth and elevation 
rotations have been accomplished. The H matrix is defined as 
(Craig, 1989, p. 46): 
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a = azimuth e = elevaticxi r - roll 



When the body moves, its H matrix relates its body 
coordinate system to the world coordinate system. The 
cumulative effect of the body's motion is transferred to the 
individual links via the H-matrix and continues to be 
transferred outboard in this manner. 
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F. INVERSE KINEMATICS 

Inverse Kinematics provides the parameter values needed to 
move the joints to a desired position. The transformation 
matrix products above are equated to the generic 
transformation matrix to make a set of nonlinear equations 
(Craig, 1989, p. 123) . 
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These equations are solved simultaneously for the unknown 
parameters (joint rotation in the case of Aquarobot) . The 
inverse kinematics of Aquarobot are solved in another thesis 
(Schue, 1993) . There are occasions when two solutions for a 
parameter are possible (Craig, 1989) . 
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G. SUMMARY 

Aquarobot's design uses rotating joints. Rotating joints 
have an advantage over sliding joints in that they generally 
provide increased dexterity. Additionally, such joints can 
usually be made smaller than sliding links (Spong, 1989) . 
They are also easier to waterproof for an underwater walking 
robot. 

Kinematics analysis permits Aquarobot's foot positions to 
be easily determined using successive transformations. 
Kinematics equations can be manipulated quickly using 
computers. Object oriented programming simplifies the 
numerous transformations necessary for an intricate multi-link 
system. Object oriented programming is discussed in the next 
chapter. 
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V. OBJECT ORIENTED PROGRAMMING 



A. INTRODUCTION 

Object Oriented Programming emphasizes the subjects which 
operations act upon in contrast to the traditional programming 
method of emphasizing the algorithms and the order necessary 
to execute them (Booch 1991) . The Object Oriented (00) 
designer constructs his modules of code based on items (known 
as objects) . These objects need not in every case accomplish 
anything significant, but they do at a minimum provide 
encapsulated data. Other designers construct their modules 
based upon the data and algorithms that are associated with 
such blocks. 

00 code permits the designer to produce elementary 
components and then link these objects together to produce a 
complex system. This parallels the thought process that 
humans use to think of objects around them. 

00 code provides two structures, object and class . 
Classes are the blueprints of a component and exist in a "kind 
of" hierarchy. Objects are the actual produced copy of the 
object (instances of classes) and may exist in a "part of" 
hierarchy in relation to other objects. 
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B. CLASS DEFINITION AND CLASS HIERARCHIES 



Classes are the building blocks or key designs of a 
system. They are synonymous with a factory's product 
blueprints. Classes provide the ability to make many modules 
(objects) that are designed identically. Each object, when 
made, provides the ’’essence" of the class (Fink, 1992) . 
Classes are static, and the information, known as fields . of 
the class are fixed. The class definition provides a template 
for the production of objects. 

A class can inherit from one or more superclasses. The 
inheriting class is known as a subclass. A class can also have 
subclasses which consists of it and additional information. 
Each senior, top level module, represents one of the most 
general designs in a system. 

Class structures may include object fields, also known as 
slots . from multiple superclasses with subclasses created 
using some priority scheme or other means to resolve conflicts 
(Booch, 1991) . These class frameworks are transferred to the 
objects that are produced. 

Multiple class structures form a design. Seldom does one 
class concisely define a system. The class hierarchy permits 
all nuances desired, regardless of significance, to be defined 
as the designer chooses at that level. 

The classes designed to serve as templates are defined as 
concrete classes. They are expected to have objects 
instantiated from them (de Paula and Nelson, 1991) . 
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Not all classes are designed as templates for actual 



object instantiation. These are called abstract classes. 
They are higher level classes which hold knowledge that all of 
their subclasses have in common. Abstract classes reduce 
duplication of common knowledge (Wu, 1991) . They are written 
so that multiple subclasses can inherit from them. 

A class capability provides two of the three features 
necessary for OO programming. First, it provides for a design 
to be defined in a generic format. Second, it allows the 
designs to be modularized at the most general level, yet still 
provides for a relationship framework where additional design 
features can be added in subclasses (Booch, 1991) . 

C. OBJECT DEFINITION AND OBJECT HIERARCHIES 

Objects are the useable products of OOP and provide the 
third capability needed for utilization of this technology. 
They are concrete software entities that can be manipulated. 
An object has all of the properties of its class. All objects 
produced from the same class contain identical fields and 
functions, yet it is important to understand that each object 
has its own identity and its own name when produced. It is by 
this name that the object is addressed within the code. The 
objects are facsimiles of the class and its behavior and 
fields. However, they may be elaborated individually (Eckel, 
1989) . 
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An object is constructed by creating an instance of the 
class desired. All superclasses and their defined functions, 
also known as methods , are available to the object. It is not 
necessary to have an object for every class. A class may have 
zero, one, or multiple instances of itself (de Paula and 
Nelson, 1991). 

An instance of an object may be produced by two different 
schemes. First, an object may be instantiated within the main 
program. This object may subsequently be addressed by its 
created name. Such an object is a top level object within the 
software. It may also be considered a subobi ect if it is 
used as part of a larger composite object. The second type of 
object, a dependent object, is instantiated directly and 
automatically as a part of another object. The dependent 
object is considered a component of the object it is 
instantiated within and has no independent name. A dependent 
object is instantiated during construction of the main object 
and is destroyed when the main object is destroyed. For 
example, a sports car, when produced, can be thought of as an 
object and its components, such as the doors, can be 
considered dependent objects since they are made during the 
sports car's construction and are legally part of the vehicle. 

Objects, when instantiated, acquire all of the fields of 
the class, but the values of these characteristics may be 
initialized individually during the construction of the 
individual objects. For example, this permits numerous 
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objects of the same class to be instantiated yet have 
different measurements or characteristics. 

An object may change its slot values during its existence. 
This provides an object with a history. An object may be 
created and destroyed. The object's functions can not be 
violated. Objects perform functions by sending requests. 
These functions are designed within the class structure and 
are applicable to the objects instantiated from the specific 
class or its class hierarchy. This capability to perform 
functions enables an object to be much more than a data 
structure . 

Functions, history, and "lifetime” characteristics provide 
objects with state, behavior, and identity (Booch, 1991) . 
This parallels their real-world counterparts. 

D. INHERITANCE 

Inheritance is defined as a "... mechanism for resource 
sharing in hierarchies" (Wegner, 1987, p. 169). It is a 
unique contribution of 00 languages. Inheritance provides an 
easy way to create objects that are very similar, although 
individual instances may have some differences (Stefik, 1986) . 
The subclass is a specialization that augments or alters the 
structure and behavior of the inherited class. It inherits all 
functions and methods defined for its superclasses. This 
includes all attributes that the superclass inherited form its 
superclass (Wegner, 1987) . A subclass may have fields or 
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methods which modify, elaborate, or add to those inherited by 
its superclass (Booch 1991) . 

When a subclass has one superclass, this is called single 
inheritance . Two or more superclasses defines multiple 
inheritance . Inheritance is a class relationship rather than 
an object relationship. 

E. CLASS AND OBJECT DIAGRAMS 

Class and object diagrams provide a logical view of a 
system. The difference between object and class diagrams is 
an important concept in 00 design. 

Class diagrams are built on interclass relationships 
involving inheritance. Superclasses and subclasses describe 
these diagrams. Class utilities provide a special 
relationship within the class diagram. A class utility is an 
abstract class type which provides functions that do not 
belong to one particular class but are accessible to all. 
Figure 5.1 displays an example of a representative class 
diagram. Note that there is only one of each class in the 
class diagram. 

Figure 5.1 displays an automobile class diagram. The top 
level module, the automobile class, is the superclass. In 
this class structure, subclasses are necessary. Two subclass 
levels are necessary in order to reach a concrete class. The 
Porsche can be produced but a sports car is an abstract class 
that can not be instantiated. 
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SUPERCLASS 




SUBCLASS 



Figure 5.1 

Example Class Diagram 



Object diagrams ”... show the existence of objects and 
their relationship in the logical design of the system ...” 
(Booch, 1991, p. 169) . An object diagram shows the 
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relationship between objects and presents each object as "part 



of” the total system. Figure 5.2 is an 
automotive system via its object diagram. 

In Figure 5.2, a Porsche is instantiated 
desired. In order to create a Porsche, 
subsystems (dependent objects) are needed, 
dependent objects are displayed. 
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Example Object Diagram 
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A composite object is defined as an object linked to 



other objects by part-of relationships. Parts of the 
composite objects may be subobjects or dependent objects. 

F. CONCLUSIONS ABOUT OBJECT ORIENTED DESIGN 

OO Programming allows designers to begin with a simple or 
general system. This principle of beginning with a large, 
less specific class or object is similar to human perception. 
First, humans determine what the overall item in questions is. 
For example, a person may look at a car. He or she is likely 
to note the model of the car at that time. Then, smaller 
"part of" subsystems of the car or specific slot values may be 
inspected. For example, the year the car was produced or the 
air conditioning system may be looked into. 00 code permits 
the programmer to define subclasses or components when they 
are needed or as the system is elaborated upon. This allows 
the programming to be accomplished in small increments. OO 
designs allow attention to be focused on the appearance and 
external capabilities of objects instead of on software 
implementation details. This prevents too much information 
from "cluttering" the user's view (Snyder, 1986). Another 
advantage to OO Programming is the capability to improve or 
alter class slots as the system changes or as corrections are 
needed. This resilience and the capability to reuse small 
subsystems in multiple objects makes 00 code economical (Booch 
1991) . 



43 



00 Programming encourages reuse of entire software class 
hierarchies. The modular design of 00 code permits new users 
to incorporate existing code without having to retest 
functions or redesign code. This extendibility of code life 
reduces a designers programming time. The modular design of 
class code permits the user to use the functions of the class 
without requiring an intimate detailed knowledge of the 
function's inner workings. 

A disadvantage of 00 Programming is that classes may be 
designed without placing functions in the most general 
superclass. This causes identical functions to be defined in 
numerous subclasses and increases complexity. Repetition 
should be minimized by placing common functions of two or more 
classes in a superclass. Of course this can be an iterative 
process with common properties or methods being factored out 
and moved upward in a class hierarchy as they are noted. 

6. SUMMARY 

Human capacity is limited in its capability to grasp 
complex systems. 00 code enables a person to look at a complex 
system as a collection of various subsystems. It also 
provides the capability to only look at areas of interest 
within an object/class. 
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Object Oriented programming provides software that is 
"malleable”. This is directly due to class structure and 
inheritance, a unique characteristic of this code (Booch, 
1991) . 

The difference between the class and object structures is 
a subtle but important one. An object is an instance of a 
class and the object may be created and destroyed within a 
program. A class is designed but it is static when a program 
is executed (Booch, 1991) . 

The long life span, maintainability, and flexibility in 
application of Object Oriented code makes it the premiere 
choice when a design with multiple subsystems is desired. The 
greatest hindrance of Object-Oriented Programming's potential 
to be the popular choice in industrial design is its current 
lack of widely excepted standards. However, there seems to be 
considerable consensus on OOP's primary concepts. The next 
chapter describes 00 languages suitable for development of an 
Aquarobot kinematic simulation. 
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VI. OBJECT ORIENTED PROGRAMMING LANGUAGES 



A. INTRODUCTION 

Not all computer languages are able to support OOP. Four 
predominant languages with OO capability are CLOS, C++, Object 
Pascal, and Smalltalk. Aquarobot is designed using CLOS and 
C++. Aquarobot 's class and object hierarchies are described 
and then created with CLOS and C++ code. 

B. DESCRIPTION OF CLOS 
1. History 

LISP evolved in the late 1950 's and was named for its 
performance method: List Processing (Winston, 1989) . The 
fundamental element in LISP is a wordlike object known as an 
atom. A group of atoms (similar to a sentence of words) is 
known as a list (Winston, 1989) . It is these lists which LISP 
is designed to manipulate. LISP allows for lists to be added 
to or deleted from indefinitely. Specific atoms may be 
extracted or manipulated using LISP created or library 
functions . 

Common LISP was officially designed in 1984 to 
accumulate the existing LISP variations into one standard 
version. This standardization was advantageous for academic 
and industrial use (Steele, 1990) . Common LISP was then 
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extended to provide 00 capability and this extension is known 
as CLOS (Common LISP Object System) (Steele, 1990) . 



CLOS (pronounced see-loss) permits each class to have 
local and shared slots. These slots can be directly accessed 
and modified by the programmer (Winston, 1989) . CLOS provides 
for multiple inheritance within the class hierarchy. 
Conflicts in multiple slot inheritance is avoided due to 
conflict precedences which define the first superclass listed 
as superior (Fink, 1992) . 

2. Benefits in the Kinematics Solution 

CLOS (and therefore LISP) has many advantages in the 
robot kinematics solution. CLOS operates in an interpretive 
environment that facilitates interactive programming, 
providing information such as variable status, with rapid 
response (Winston, 1989) . This capability for immediate 
answers to a drafter's questions provides ease in debugging 
as well as the drafting of programs (Winston, 1989) . Lists 
are addressed and manipulated using programmer defined 
symbolic names which generally tend to decrease the code 
length and improve readability (Keene, 1989) . 

The symbol manipulation and interactive capability of 
CLOS simplify the kinematics solution. Each limb's joints can 
be placed in one list or each parameter can be designed as an 
atom in a joint list. CLOS code provides compact code for 
extensive systems as well as functions that are easy to read. 
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More generally, academic institutions and industries can 
create complex systems with big programs that run faster and 
are less expensive due to the compact code (Winston, 1989) . 

CLOS also incorporates the natural language 
orientation of LISP. Class and object structures and their 
slots and values are easily understood (Keene, 1989) . 

C. DESCRIPTION OP C++ 

1. History 

C++ was designed in 1986 at AT&T Bell Laboratories by 
Bjarne Stroustrup, and is a superset of the C language (Booch, 
1991) . C++ incorporates the programming abilities of C and 

adds 00 properties as well as type checking and operator 
overload functions. In 1989, C++ Version 2.0 provided 
multiple inheritance (Stroustrup, 1991) . 

2. Benefits in the Kinematics Solution 

C++ is similar in format to many presently popular 
languages such as Ada and C. The familiar format is an 
advantage of C++ when an OOP language is necessary. 
Conversely, LISP has a unique format that is not currently 
a popular choice in academic institutions or industry. This 
uniqueness is a disadvantage to CLOS unless the drafter 
understands LISP. 

C++ permits users to apply functions without 
necessitating intimate detailed knowledge of the class inner 
workings (Booch, 1991) . C++ uses a header file to provide a 
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top level view of class structure and the functions which 
apply (Booch, 1991) . Functions must be defined for a specific 
class within its hierarchy because of C-i-+'s strong typing. 
Subclasses may alter functions that their superclass defines. 
Common operators such as addition (+) and equality (=) are 
generically defined for common classes (e.g., integer, double, 
array, etc.), but they must be redefined in new classes where 
their use is desired (e.g., a matrix class or link chain) 
(Ammeraal, 1991) . 

D. SMALLTALK AND OBJECT PASCAL DESCRIPTIONS 

Smalltalk and Object Pascal are also OO languages. Like 
C++ and CLOS, Object Pascal provides an enhancement of the 
Pascal language. Object Pascal was specifically designed to 
add an OO capability to Pascal. However, Object Pascal is 
more restrictive than C++ in code development (Booch, 1991) . 
All class slots are public so slots may be changed while 
performing another class function (Booch, 1991) . 

Smalltalk was designed as a pure OO language and provides 
many predefined classes. Unlike Object Pascal, all slots in 
Smalltalk are private. Object Pascal is unique in that it 
provides an overall system template and all created classes 
are considered subclasses of a predefined superclass called 
"Object” (Booch, 1991) . Smalltalk is not a strongly typed 
system, therefore a compiler cannot optimize the code. 
Smalltalk is limited to single inheritance (Booch, 1991) . 
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E. SUMHARY 

OO capability is presently a popular (and seemingly 
necessary) addition to current programming languages. This 
development coincides with the increased use of OO in academic 
and industrial system design. 

CLOS provides an easier format for user's to read than 
C++. However, C++ provides non-list manipulations which are 
often more efficient. Both languages require knowledge of the 
original language they embellished or a similarly formatted 
language. CLOS requires less code space than C++, but C++ 
usually executes more efficiently, and may require less 
memory. 

CLOS provides dynamic memory allocation and uses "garbage 
collection" to accumulate unused memory space. Activity is 
suspended during garbage collection which may hinder real time 
calculations in some garbage collection methods. In contrast, 
C++ uses a memory heap which requires that memory be removed 
and then explicitly returned to the heap when the memory space 
is no longer needed. The next chapter provides a description 
of CLOS and C++ and examples of their formats in the context 
of the Aquarobot code developed in this thesis. 
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VII. AQUAROBOT CODE DESCRIPTION 



A. INTRODUCTION 

In order to produce an Aquarobot simulation, each of the 
robot's major parts needed to be simulated. OOP was chosen as 
the best method to achieve this goal. One version of 
Aquarobot was written in CLOS by Prof. Robert McGhee at the 
Naval Postgraduate School. The other version was written in 
C++ by this author. In this chapter of this thesis, the 
object and class diagrams for these two inplementations are 
presented along with examples of the method each language uses 
to produce an individual class and instantiate an object. The 
C++ graphics code is discussed and examples of the display are 
included. The complete CLOS and C++ Aquarobot programs are 
found in Appendix A and B respectively. 

B. AQUAROBOT CLASS AND OBJECT HIERARCHIES 

The class hierarchies designed to produce an Aquarobot in 
CLOS and C++ are shown in Figures 7.1 and 7.2 respectively. 
These two figures are not identical, but there are major 
portions that are similar. 

The RigidBody class is a superclass of the system. Its 
subclasses are the major pieces with which Aquarobot and its 
components are created. The AquaLeg class uses the Link 
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CLOS Aquarobot Class Diagram 
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C++ Aquarobot Class Diagram 
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subclasses (linkO through link3) and a few numerical slots to 
create another top level class. The AquaLeg class also 
possesses functions that manipulate an AquaLeg type. The 
Matrix class is unique to the C++ version. The CLOS version 
uses lists to store data while the C++ version uses this 
defined Matrix class and its functions to store and manipulate 
the data. The Matrix class is a typical example of a class 
utility. 

The object hierarchy used to instantiate an Aquarobot 
differs in the two versions. Figure 7.3, the C++ object 
diagram, constructs Aquarobot as seven subobjects which can be 
deleted or reproduced without affecting the existence of the 
other. Figure 7.4 displays the CLOS object diagram that has 
one top level object with dependent object hierarchy 
containing a total of thirty-one objects. The Leg object and 
its dependent subobjects are identical in each language 
version. 

C. AQUAROBOT CLASS DEFINITION CODE 

Classes are defined in various ways dependent upon the 00 
language used. There are, however, many similarities in class 
attributes. For example, both CLOS and C++ have slots for the 
items within a class. The AquaLeg class definition in both 
language versions is explained in the following paragraphs. 
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C++ Object Hierarchy 
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1. CLOS Class Description 



CLOS provides a template that contains both optional 
and mandatory information requirements. This template can 
be found in CLOS manuals. Figure 7.5 is the aqua-leg class 
defined using CLOS. Eight slots are defined and then 
initialized using the ; initara or ; initform command. The 
dependent objects shown in Figure 7.4 are instantiated 
within the aqua-leg class as linkO through links using the 
make-instance command. The linkO class, for example, 
incorporates the superclass slots of Link. 

The functions related to the aqua-leg class are 
defined outside of the class definition in defmethods . 
"Initialize-leg” is a function which requires an "aqua-leg” 
and an "aquarobot-body" class as input. Each input is given 
a local variable name of "leg" and "body" respectively. The 
functions may call other functions or change slot values. 
The CLOS code includes a camera class because the code was 
developed on a Sun workstation while the C++ version uses 
the graphics library on a Iris workstation. 

2. C++ Class DESCRIPTION 

The C++ AquaLeg class is defined within the AquaLeg.H 
file (Figure 7.6). Like the CLOS version in Figure 7.5, there 
are four dependent objects that are slots of the AquaLeg 
class. Like the CLOS version, functions which are applicable 
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(defclass aqua-leg () 

( (leg-attachment-angle 

:initarg : leg-attachment-angle 
: accessor leg-attachment-angle) 

(linkO 

rinitform (make-instance 'linkO) 
raccessor linkO) 

(linkl 

rinitform (make-instance 'linkl) 
raccessor linkl) 

(link2 

rinitform (make-instance 'link2) 
raccessor link2) 

(links 

rinitform (make-instance 'linkS) 
raccessor linkS) 

(motion-complete-f lag 
rinitform nil 

raccessor motion-complete-flag) 

(previous-foot -posit ion 
rinitform nil 

raccessor previous -foot -posit ion) 

(cur rent -foot-posit ion 
rinitform nil 

raccessor current-foot -position) ) ) 

(defmethod initialize-leg (deg aqua-leg) (body aqua robot-body) ) 
(setf (inboard-link (linkO leg)) body) 

(setf (inboard-link (linkl leg)) (linkO leg)) 

(setf (inboard-link (link2 leg)) (linkl leg)) 

(setf (inboard-link (linkS leg)) (link2 leg)) 

(rotate-link (linkO leg) (leg-attachment-angle leg)) 
(rotate-link (linkl leg) (inboard- joint-angle (linkl leg))) 
(rotate-link (link2 leg) (inboard- joint-angle (link2 leg))) 
(rotate-link (linkS leg) (inboard- joint-angle (linkS leg))) 
(setf (current-foot-position leg) 

(near 3 (first (t ransformed-node-list (link3 leg)))))) 



Figure 7.5 

CLOS Code Excerpt Defining and Implementating 
Aquarobot Leg Kinematics 
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class AquaLeg 
public : 

// these dependent objects are instantiated 
LinkO *linkO; 

Linkl *linkl; 

Link2 *link2; 

Link3 *link3; 

// the flag is set to 1 if the motion is completed without 
// reaching any link limits 
int motion_complete_f lag; 

// the flag is set to 1 if the leg is on the ground 
int leg_support__f lag; 

// the angle off of leg one where the leg is attachec to 
// the body 

double leg_attachment_angle; 

AquaLeg (AquarobotBody&, double); // constructor and initializer 
"AquaLegO; // destructor 

void Move Incremental (Aqua robotBody double deltal, double delta2, 

double delta3) ; 

double GetLegAttachmentAngle {) { return leg_attachment_angle; ) 

int GetMotionCompleteFlag 0 ( return motion_complete_f lag; ) 

void SetLegAttachment Angle (double angle) ( leg_attachment_angle = angle;} 

void SetMotionCompleteFlag (int flag) (motion_complete_f lag = flag;} 

int GetLegSupportFlag 0 ( return leg_support_flag; } 

void SetLegSupportFlag (int flag) ( leg_support_f lag = flag;} 

}; 

#endif 



Figure 7.6 

C++ Code Excerpt Defining Aquarobot 
Leg Kinematics 
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to the AquaLeg class are included within the class definition. 
An example, shown in that figure, is the "Move Incremental" 
function which increments the joint angles of a specified leg 
by a given amount. 

The C++ function similar to the CLOS "initialize-leg" 
function is the C++ constructor "AquaLeg" shown in Figure 7.6. 
This function requires an AquarobotBody class and a double 
number as inputs. This and the other AquaLeg functions are 
defined within the AquaLeg. C file (Figure 7.7) . Like the CLOS 
version, it is within the constructor that the dependent 
objects, LinkO through Link3, are instantiated. Similar to 
the CLOS version, other functions may be called or slot values 
altered. The "matrix" class, found within the MatrixMy.C and 
MatrixMy.H files (see Appendix C) , is a class utility and is 
used within the AquaLeg constructor. 

D. AQUAROBOT OBJECT INSTANTIATION CODE 

Objects may be constructed in various composition within 
an OOP. However, the method of actually instantiating an 
object varies among 00 languages. The CLOS version, shown in 
Figure 7.4, displays one top level object while the C++ 
program, shown in Figure 7.3, makes seven subobjects to 
produce one Aquarobot system. This section will discuss the 
individual language's method of instantiation using the two 
Aquarobot versions. 
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! I *********************************************************** 

// FUNCTION: -AquaLegO 

// PURPOSE: destructor of AquaLeg class 

II *********************************************************** 

AquaLeg : : -AquaLeg ( ) 

{ 

delete linkO; 
delete linkl; 
delete link2; 
delete link3; 

} 

II *********************************************************** 

// FILENAME: AquaLeg 

// PURPOSE: constructor of AquaLeg class 

// RETURNS: AquaLeg class with values 

II ************************************************************ 

AquaLeg :: AquaLeg (Aqua robotBody Sbody, double angle) 

{ 

motion_complete_f lag =1; // initializes flag value 

SetLegAttachmentAngle (angle) ; 

linkO = new LinkO; 

linkl = new Linkl; 

link2 = new Link2; 

link3 = new Link3; 



// initial link values initialized 

// temp matrix adds in the T_matrix needed for the physical 
// attachment of the leg to the body 
matrix temp; 

// updates the Transformation matrix from body center to the 
// leg attachment point 

temp.UpdateTMatrix (GetLegAttachmentAngle ( ) , 0 . , 0 . , 0 . ) ; 
temp = *body . H_matrix * temp; 

linkO->RotateLink (Stemp , linkO->Get InboardJointAngle ( ) ) ; 

linkl->RotateLink ( linkO->H_jnatrix, linkl->Get InboardJointAngle () 
link2->RotateLink (linkl->H_matrix, link2->Get InboardJointAngle ( ) ) 
link3“>RotateLink ( link2->H_matrix, link3->Get InboardJointAngle () ) 

} 



Figure 7.7 

C++ Code Excerpt Implementing Aquarobot 
Leg Kinematics 
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1. CLOS Object Description 

The CLOS version produces an Aquarobot by performing 
the function **aqua-picture” in the LISP screen environment. 
This function's code is displayed in Figure 7.8 and 
instantiates a top level object, Aquarobot, (named ”aqua-l”) 
in its first line using the make-instance command. The class 
"aquarobot" is used as the blueprint for this instantiation. 
This class consists of one body and six legs ("legl" through 
"leg6") as dependent objects. These slots are instantiated 
using the same make-instance command when an "aquarobot" is 
created. Slot values are instantiated within the aqua-leg 
instantiation using the variable ; lea-attachment-anale which 
was the initializing argument for a slot with the same name in 
the aqua-leg class (Figure 7.5). 

2. C+-I- Object Description 

Bot.C (Figure 7.9) is the main program the C++ 
version. This program controls the construction of the 
Aquarobot. The AquarobotBody and six AquaLegs are 
instantiated at this top level and they are all subobjects 
since there is no explicit Aquarobot instantiated. Similar to 
CLOS, each object is given a name (for example "leg3") and 
initialization values at the same time it is instantiated. 
C++ does not provide a command that equates to CLOS's make- 
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(defclass aquarobot 
( (body 

rinitform (make 
: accessor body) 
(legl 

;initform (make 
:accessor legl) 
(leg2 

rinitform (make 
raccessor leg2) 
(leg3 

rinitform (make- 
raccessor leg3) 

( leg4 

rinitform (make- 
raccessor leg4) 
(legs 

rinitform (make- 
r accessor legS) 
(leg6 

rinitform (make- 
raccessor leg6) ) 



() 

instance 'aquarobot- 

instance ' aqua-leg 

instance 'aqua-leg 

instance 'aqua-leg 

instance 'aqua-leg 

instance 'aqua-leg 

■instance 'aqua-leg 

) 



■body) 

leg-attachment- 

leg-attachment- 

leg-attachment- 

leg-attachment- 

leg-attachment- 

leg-attachment- 



angle 

angle 

angle 

angle 

angle 

angle 



(deg-to-rad 0)) 
(deg-to-rad 60)) 
(deg-to-rad 120)) 
(deg-to-rad 180)) 
(deg-to-rad 240)) 
(deg-to-rad 300) ) 



(defmethod initialize ( 
(transform-node- list 
(initialize-leg (legl 
(initialize-leg (leg2 
(initialize-leg (leg3 
(initialize-leg (leg4 
(initialize-leg (legS 
(initialize-leg (leg6 



(aqua aquarobot) ) 
(body aqua) ) 
aqua) (body aqua) ) 
aqua) (body aqua)) 
aqua) (body aqua)) 
aqua) (body aqua) ) 
aqua) (body aqua) ) 
aqua) (body aqua))) 



(defun aqua-picture () 

(setf aqua-1 (make-instance 'aquarobot)) 
(initialize aqua-1) 

(setf camera-1 (make-instance 'camera)) 
(create-camera-window camera-1) 
(take-picture camera-1 aqua-1) ) 



Figure 7.8 

CLOS Code For Aquarobot Class 



main ( ) 

{ 



/* value returned from the event queue */ 

short value; 
long mainmenu; 

long hititem; 

FILE *ifp; 

ifp - fopen (”bot .dat", "r”) ; 

/* initialize the IRIS system */ 
initialize () ; 

/* Create Pop Up Menus */ 
mainmenu “ makethemenus ( ) ; 

// make the robot from its pieces 
Aqua robot Body aquabody; 

AquaLeg legl (aquabody, 0 . 0) ; 

AquaLeg leg2 (aquabody, 60 . 0) ; 

AquaLeg leg3 (aquabody, 120 . 0) ; 

AquaLeg leg4 (aquabody, 180 . 0) ; 

AquaLeg legS (aquabody, 240 . 0 ) ; 

AquaLeg leg6 (aquabody, 300.0); 



Figure 7.9 

C++ Code Excerpt From Main Program Showing 
Instantiation of the Parts of Aquarobot 
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instance command. Instead, C++ instantiates an object by 
declaring the class and providing a name and information 

necessary. 

E. GRAPHICS 

1. Graphics Display 

The CLOS version of Aquarobot (Appendix A) was 
graphically simulated on a low end Silicon Graphics Indigo 
graphics workstation using a LISP camera object. This is the 
camera. cl file in Appendix A and it was created as a debugging 
tool because CLOS does not provide a graphics capability 
within its library. Examples of the CLOS graphics produced by 
a camera object are in Appendix C. The C++ version of 

Aquarobot (Appendix B) was simulated on the same workstation 
as the CLOS code. The C++ code uses the system's basic 

graphic library, gl. The C++ graphics code will be discussed 
in this section. 

The C++ simulation was developed to support the 
debugging of control software. A user's manual explaining its 
use in this application has been produced (Suzuki, 1993) . The 
graphics code is included in bot.C in Appendix B. This file 
includes the main program (a requirement of C++) which 

provides the initial instantiation of Aquarobot and controls 
function calls. This file also provides the graphics setup 
(in the initialize function) and the function that draws the 
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stick figure Aquarobot (in the drawaqua function) . This 
coordination of bot.C is depicted in the flow diagram in 
Figure 7.10. 

Aquarobot is instantiated in the reset position. 
Figure 7.11 displays this first graphical view. The next 
motion for Aquarobot is provided from the output of the gait 
algorithm. The information is provided by the position and 
orientation change of the body and the change in joint angle 
for each joint of each leg. The respective changes are 
transferred to the body's Moveincremental function and leg 
Moveincremental function. These functions update desired body 
and link positions. The body's function is called first 
because each leg uses the body's updated H matrix computation 
in their functions. The function FindPositions determines the 
Cartesian coordinate location of each joint and the footpad 
for each leg, as well as the body's position and orientation. 
Figure 7.12 shows a change ordered in one of leg one's joint 
angles. The C++ graphics code continuously polls for an 
acceptable queued signal. This signal determines the path 
taken and the functions performed within that option. 

The CLOS version uses the user interface on the 
terminal as its main program. As shown in the script file of 
Appendix C, this method does not use an explicit continuous 
polling loop like C++. Rather, LISP provides an infinite 
read-eval-print loop within its user interface. Appendix C 
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Figure 7.10 

C++ Program Flow Diagram for Main Program 
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Figure 7.11 

Initial Aquarobot Graphic Display 
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also provides examples of Aquarobot graphics obtained using 
the code in Appendix A. 

2. User Interface 

In the C++ version of the Aquarobot simulation code, 
one acceptable queue signal is the clicking of an option on 
the menu shown upon the screen. Figure 7.12 displays the menu 
and its options. The options provide various camera views of 
Aquarobot and the ability to read from a data file that 
consists of data changes. The camera views are particularly 
helpful in the debugging of gait motions conducted in the gait 
algorithm function. 

F. SUMHARY 

C++ and CLOS are both similar in their method of defining 
a class. The instantiation technique differs between the 
languages with C++ requiring a class constructor function that 
defines the instantiation while CLOS uses a reserved command 
and the class definition. 

The Aquarobot programs were not designed from identical class 
and object hierarchies and this provides varied examples of 
design as well as OO language variations. Appendices A and B 
include the entire CLOS and C++ codes. Appendix C provides a 
script file and graphical pictures produced using the CLOS 
version. The next chapter evaluates the CLOS and C++ codes 
and their performance. 
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Figure 7.12 

Ordered Motion Aquarobot Graphic Display 





VIII. EVALUATION 



A. INTRODUCTION 

Both the CLOS and C++ versions were successful in 
producing a graphic simulator of Aquarobot. Each code, 
however, varies in its graphical performance and code 
characteristics . 

B. CLOS/C++ CODE EVALUATION 

The CLOS code usually requires less lines of code to write 
a function than C++. The codes in Appendices A and B display 
approximately a three to one ratio of length in favor of CLOS. 
This compact code provides CLOS an advantage in 
understandability and prototyping. Unfortunately, CLOS 
function definitions, when optimized for conciseness, may be 
rather cryptic. C++, although longer in length, is similar in 
format to many more common languages, and therefore it is in 
some ways easier to read, especially by programmers not 
skilled in CLOS and LISP. 

CLOS provides dynamic memory allocation and uses garbage 
collection to accumulate unused memory space. Activity is 
suspended during garbage collection methods. In contrast, C++ 
uses a memory heap which requires memory to be removed and 
then explicitly returned to the heap when the memory space is 
no longer needed. 
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C++ and CLOS are both similar in their method of defining 
a class. The instantiation technique differs between the 
languages with C++ requiring a class constructor function that 
defines the instantiation while CLOS uses a reserved command, 
make-instance . and the class title. 

C. CLOS/C++ GRAPHICAL EVUUATION 

Both Aquarobot versions were simulated with the same leg 
and body motions. The time required to calculate new joint 
parameters via kinematics was recorded. The compiled CLOS 
version requires 310 ms for execution and display of one move 
(six degrees of freedom for body motion and 18 leg joint 
motions) while 160 ms is required for the same result using 
the C++ version. 

The numerical results above were run without either code 
being optimized, nor were compiler switches set for optimized 
code generations. The C++ version performed faster than the 
CLOS version by a factor of two to one. The C++ speed 
advantage was not noteworthy enough to recommend against using 
CLOS unless execution efficiency is the dominant factor. In 
addition, CLOS required less programming effort (i.e., more 
compact code) than C++. This author recommends CLOS as a 
prototype executable specification code when a complex systems 
are involved. However, the common format and familiarity of 
C++ provides a better comprehension of the code by others who 
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might use or modify it (which are major advantages) . In 
addition, it appears that C++'s two to one speed ratio may be 
improved with optimization. 

D. SUMMARY 

The C++ and CLOS Aquarobot objects have been designed 
using classes that are common to all rigid body manipulators. 
The generic classes are defined in both code versions and can 
be incorporated into other robot design code. This 
reusability of 00 code as well as its simple modification 
steps are advantages to this design in this and other research 
projects, since future researchers may build on current work. 
This author's C++ code is currently being used in Aquarobot 
dynamics research for a dissertation at Ohio State University 
(McMillan, 1992). 

The C++ code is also being used for its original purpose 
as a simulator to debug control algorithms. Specifically, 
Master degree students at the Naval Postgraduate School are 
currently using this simulator to investigate gait algorithms 
(Schue, 1993) . The graphical simulator provides a useful and 
time saving method to check this work visually. It also 
provides an environment where innovative methods for motion 
can be tried without requiring the physical robot to be put 
into danger. The last chapter of this thesis provides 
conclusions concerning this research project and suggests 
topics for future research. 
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IX. CONCLUSIONS 



A. INTRODUCTION 

This thesis represents the beginning of a major research 
effort to provide a generic walking robot simulator, although 
the concepts developed here can be used for any articulated 
rigid body system The overall research project is centered 
around Aquarobot, a six-legged walking machine developed in 
Japan. While the techniques and the computer programs 
developed and used here are only a small part of the overall 
effort of the Naval Postgraduate School Aquarobot project, 
they represent critical first steps. 

B. FUTURE USE OF CODE IN OTHER ROBOT DESIGNS 

The codes discussed in this thesis and included in 
entirety in Appendices A and B provide the basic classes 
necessary to create any rotary link manipulator objects. The 
C++ code (Appendix B) provides the core structure that any 
design requires to produce a graphic simulator using Silicon 
Graphics Iris systems. The CLOS version (Appendix A) includes 
a camera class that can be used on any SUN workstation as well 
as any Iris system which has been provided with a LISP 
compiler. 
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C. FUTURE USE OF AQUAROBOT 

Aquarobot's underwater walking capability provide many 
possibilities for its future use. Aside from its original 
application to assist with the quality control and 
construction supervision for tsunamai sea wall foundations, 
other possibilities include its use at marinas to locate large 
items dropped into the water, and at lakes or shallow beaches 
when an underwater search is needed. Aquarobot could also be 
helpful in detecting underwater cracks in piping such as 
electric cables and gas lines. 

Additionally, the Aquarobot concept presents many possible 
uses in military applications. For example, an aquatic 
walking machine provides an alternative method of mine 
detection on the sea floor or in a surf zone. Upon completion 
of an analysis of the feasibility of walking between land and 
water, it may be found that this class of robots could be used 
for beach inspection prior to an amphibious landing, thus 
saving lives. 

D. FUTURE RESEARCH IDEAS 

This thesis describes the initial stage of the Aquarobot 
research project. There are a number of avenues of future 
reasearch. Some of these areas include: simulating the 
robot's dynamics, modeling the joint motors, modeling 
Aquarobot's hinged foot pads, modeling the hydrodynamic 
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effects of legged walking machines, improving the graphic 
portion, and gait planning research. 

The graphics area alone provides a number of areas of 
research such as: providing collision detection and creating 
an Aquarobot replica that is more faithful in appearance to 
the actual robot. Both graphics codes currently display a 
stick figure. 

Aside from this thesis' use of CLOS as executable 
specification code for a C++ final version, CLOS could also be 
used in incremental development of C++. This would take 
advantage of CLOS's superior debugging environment. Another 
alternative in code production would be to use CLOS as the 
main program and import C++ functions. This may in the end 
prove to be the best way to construct an interactive 
simulator, but this has yet to be investigated. 

E. SUMMARY 

Robotics engineers have made impressive progress in 
accomplishing the goal of producing an effective and practical 
walking machine. Such a machine will permit society to 
achieve functions and carry out missions not presently 
possible. Also, they will improve current methods of 
performing tasks we do now but not very efficiently, or with 
considerable danger to human workers. 

The legged robot concept is in its infancy. It is vital 
that we have simulation tools capable of providing an 
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accurate model of complicated linkage mechanisms used for 
manipulation and locomotion. These tools reduce the need for 
a prototype vehicle in the early stages of development, and 
provide a safe environment to attempt unusual and possibly 
dangerous tests. Trying untested gaits, designs, etc. in a 
simulation environment allows for better understanding of the 
performance envelope and capabilities of a walking machine, 
saves money, and potentially saves lives. 

Simulation tools such as modeling algorithms, powerful 
computer languages, and graphical capabilities enhance and 
promote technology. There are many ways to build a simulation 
model, but the use of the combination of kinematic modeling, 
object oriented languages, and graphically equipped computer 
systems offers a flexible and robust design method for both 
large complex robots and simple one limb imitations of 
organisms . 

Aquarobot is a six-legged walking robot whose software 
will be improved using this simulation model. Later 
generations of legged machines may also benefit from similar 
simulation studies. Aquarobot' s actions and stability in 
various postures and with different gaits will be tested to 
enhance its efficiency and design. Hopefully, through these 
tests and improvements, legged machines will be able to 
exploit their many advantages in areas where wheeled vehicles 
are now used, as well as permitting access to areas where no 
vehicle can now go. 
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APPENDIX A - CLOS CODE 



(defclass link (rigid-body) 

( (motion-limit-f lag 
rinitform nil 

iaccessor motion-limit-flag) 
(twist-angle 
:initarg :twist-angle 
:accessor twist-angle) 

( link-length 
;initarg ; link-length 
saccessor link-length) 

( inboard- joint -angle 
: initarg ; inboard- joint-angle 
.-accessor inboard- joint -angle) 

( inboard- joint-displacement 
; initarg r inboard- joint -displacement 
: accessor inboard- joint-displacement ) 
( inboard- link 
.-initarg : inboard- link 
:accessor inboard-link) 

(A-matr ix 

:accessor A-matrix) ) ) 

(defclass rotary-link (link) 

( (min- joint-angle 

rinitarg :min- joint-angle 
taccessor min- joint-angle) 

(max- joint -angle 
: initarg :max- joint-angle 
:accessor max- joint-angle) ) ) 

(defclass sliding-link (link) 

( (min- joint-displacement 

-. initarg :min- joint-displacement 
taccessor min- joint-displacement ) 
(max- joint -displacement 
: initarg :max- joint-displacement 
taccessor max- joint-displacement) ) ) 
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•qua 'link. cl 

(deCclass linkO ( rotary- 1 ink ) 

((twist-angle :initform 0) 

(link-length rinitform 37.5) 

( inboard- joint-angle :initform 0) 

( inboard- joint-displacement ;initform 0) 

(min- joint-angle tinitform (deg-to-rad -3601) 

(max-joint-angle ;initform (deg-to-rad 360)) 

(node-list sinitform '((0 001) (0001) (-37.5 0 0 1))) 
(polygon-list :initCorm '((1 2))))) 

(defclass linkl (rotary-link) 

((twist-angle sinitform (deg-to-rad -90)) 

(link-length tinitform 20) 

( inboard- joint-angle tinitform 0) 

( inboard- joint-displacement tinitform 0) 

(min- joint -angle tinitform (deg-to-rad -60)) 

(max- joint-angle tinitform (deg-to-rad 60)) 

(node-list tinitform '((0001) (000 1) (-20 0 0 1))) 

(polygon-list tinitform '((1 2))))) 

(defclass link2 (rotary-link) 

((twist-angle tinitform 0) 

(link-length tinitform 50) 

( inboard- joint-angle tinitform (deg-to-rad 66.4)) 

( inboard- joint-displacement tinitform 0) 

(min- joint-angle tinitform (deg-to-rad -106.6)) 

(max- joint-angle tinitform (deg-to-rad 73.4)) 

(node-list tinitform '((0 001) (0001) (-50 0 0 1))) 
(polygon-list tinitform '((1 2))))) 

(defclass link3 (rotary-link) 

((twist-angle tinitform 0) 

(link-length tinitform 100) 

( inboard-joint-angle tinitform (deg-to-rad -156.4)) 

( inboard- joint-displacement tinitform 0) 

(min- joint-angle tinitform (deg-to-rad -156.4)) 

(max- joint -angle tinitform (deg-to-rad 23.6)) 

(node-list tinitform '((0 001) (0001) (-100 0 0 1))) 
(polygon-list tinitform '((1 2))))) 

(defmethod update-A-matr ix ((link link)) 

(with-slots (twist-angle link-length inboard- joint -angle 
inboard- joint -displacement A-matrix) link 
(setf A-matrix (dh-matrix (cos inboard- joint-angle) 

(sin inboard- joint-angle) (cos twist-angle) (sin twist-angle) 
link-length in)aoard- joint-displacement ) ) ) ) 

(defun deg-to-rad (angle) (* .017453292519943295 angle)) 

(defmethod rotate ((link rotary-link) angle) 

(setf ( inboard- joint-angle link) angle) 

(update-A-matr ix link) 

(setf (H-matrix link) (matrix-multiply (H-matrix ( inboard- link link)) 

(A-matrix link) ) ) 

(transform-node-list link)) 

(defmethod rotate-link ((link rotary-link) angle) 

(cond ((> angle (max- joint-angle link)) 

(rotate link (max- joint-angle link)) 
(setf (motion-limit-flag link) t)) 
((< angle (min- joint-angle link)) 

(rotate link (min- joint-angle link)) 
(setf (motion-limit-flag link) t) ) 

(t (rotate link angle) (setf (mot ion-1 imit- flag link) nil)))) 
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a<iua-l«g.el 



(defclass aqua-leg () 

( ( leg-attachment-angle 

:initarg : leg-attachment -angle 
:accessor leg-attachment-angle) 
(linkO 

:initform (make-instance 'linkO) 
:accessor linkO) 



(linkl 

rinitform (make- instance 'linkl) 
laccessor linkl) 



( link2 

:initform (make -instance 'link2) 
:accessor link2) 

(linkl 

:initform (make-instance 'link!) 
:acces3or linkl) 

(mot ion-complete- flag 
tinitform nil 

:accessor motion-complete-flag) 
(previous -foot -posit ion 
tinitform nil 

: accessor previous - f oot -pos it ion ) 
(current -foot -posit ion 
tinitform nil 

taccessor current-foot-position) ) ) 



(defmethod initialize-leg ((leg aqua-leg) (body aqu a robot -body ) ) 

(setf (inboard-link (linkO leg)) body) 

(setf (inboard-link (linkl leg)) (linkO leg)) 

(setf (inboard-link (link2 leg)) (linkl leg)) 

(setf (inboard-link (linkl leg)) (link2 leg)) 

(rotate-link (linkO leg) (leg-attachment-angle leg)) 

(rotate-link (linkl leg) ( inboard- joint -angle (linkl leg))) 

(rotate-link (link2 leg) ( inboard- joint -angle (link2 leg))) 

(rotate- link (linkl leg) ( inboard- joint-angle (linkl leg))) 

(setf (current -foot -pos it ion leg) 

(near 1 (first (transformed-node-list (linkl leg)))))) 

(defmethod take-picture ((camera camera) (leg aqua-leg)) 

(take-picture camera (linkl leg)) 

(take-picture camera (link2 leg)) 

(take-picture C 2 unera (linkl leg))) 

(defmethod move- incremental ((leg aqua-leg) increment-list) 

(rotate-link (linkO leg) ( leg-attachment -angle leg)) 

(rotate-link (linkl leg) 

(■*• (first increment- list) (inboard- joint -angle (linkl leg)))) 
(rotate-link (link2 leg) 

{* (second increment-list) ( inboard- joint-angle (link2 leg)))) 
(rotate-link (linkl leg) 

(•*. (third increment-list) ( inboard- joint -angle (linkl leg)))) 
(setf (previous-foot-position leg) (current -foot -pos it ion leg)) 

(setf (current -foot -pos it ion leg) 

(near 1 (first (transformed-node-list (linkl leg))))) 

(setf (motion-complete-f lag leg) (not (or (mot ion- limit -flag (linkl leg)) 

(motion-limit-flag (link2 leg)) (motion-limit-flag (linkl leg)))))) 



(defmethod f eas ible-movep ((leg aqua-leg) allowable-sinkage allowable -slippage) 
(and (<s (third (current -foot -pos it ion leg)) allowable-sinkage) 

(or (minusp (third (cur rent -foot -pos it ion leg))) 

(minusp (third (previous -foot -pos it ion leg))) 

(<s (vector- length (vector-slippage leg)) allowable-slippage)))) 



(defmethod vector-slippage ((leg aqua-leg)) 



(vector-subtract (rest (reverse (previous-foot -position leg))) 
(rest (reverse (current-foot-position leg))))) 
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aqua. el 



1 



(d«f class aquarobot -body (rigid-body) 

( (node -list 

rinitform '((0 001) (37.5 001) (18.75 32.48 0 1) 

(-18.75 32.48 0 1) (-37.5 001) (-18.75 -32.48 0 1) 
(18.75 -32.48 0 1) (37.5 0 -15 1))) 

(polygon -list 

tinitform '((123456) (17))) 

(H-tnatrix 

linitform (homogeneous-transform 00000 2 -init)))) 



(defclass aquarobot () 

( (body 

rinitform (malce-instance 
:accessor body) 

(legl 


'acjuarobot- 


-body) 






: initform 
: accessor 
(leg2 


(malce-instance 

legl) 


'aqua -leg 


: leg-attachment -angle 


(deg-to-rad 


0) ) 


rinitform 

raccessor 

(leg3 


(ma)ce- instance 
leg2) 


'aqua -leg 


: leg -attachment -angle 


(deg-to-rad 


60) ) 


: initform 
raccessor 
(leg4 


(make- instance 
leg3) 


'acfua-leg 


: leg-attachment-angle 


(deg-to-rad 


120) ) 


rinitform 
raccessor 
( leg5 


(make- instance 
leg4) 


'aqua -leg 


: leg-attachment-angle 


(deg-to-rad 


180)) 


r initform 
raccessor 
(leg6 


(make- instance 
legS) 


'acjua-leg 


: leg-attachment -angle 


(deg-to-rad 


240) ) 


r initform 
raccessor 


(make-instance 

leg6))) 


'acjua-leg 


: leg-attachment -angle 


(deg-to-rad 


300)) 



(defmathod initialize ((aqua aquarobot)) 

( transform-node- list (body aqua)) 
(initialize-leg (legl aqua) (body aqua)) 

( initialize-leg (leg2 aqua) (body aqua)) 

(initialize-leg (leg3 aqua) (body aqua)) 

(initialize-leg (leg4 aqua) (body aqua)) 

(initialize-leg (legS aqua) (lx>dy aqua)) 

(initialize-leg (leg6 aqua) (body aqua))) 



(defun aqua-picture () 

(setf aqua-1 (ma)ce-instance 'aquarolsot) ) 
(initialize aqua-1) 

(setf camera-1 (malce-instance 'camera)) 
(create-camera -window camera-1) 

( ta)ca-picture camera-1 aqua-1)) 



(defmethod ta)ce-picture ((camera camera) (aqua aquarobot)) 
(ta)ce-picture camera (body aqua)) 

(talce-picture camera (legl aqua)) 

(ta)ce-picture camera (leg2 aqua)) 

(ta)ce-picture camera (leg3 aqua)) 

(ta)ce-picture camera (leg4 aqua)) 

( ta)ce-picture camera (leg5 aqua)) 

(ta)ce-picture camera ( leg6 aqua))) 



(defun new-picture () 

( ta)ce-picture camera-1 aqua-1)) 

(defconstant z-init -54.181866) 



(defmethod move- incremental ((aqua aquarobot) increment- list ) 
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aqua. el 



(mov®-incr«m«ntal 
(mov® - incramenta 1 
(mov®- incremental 
(move- incremental 
(move- incremental 
(move- incremental 
(move- incremental 



(body aqua) 
(legl aqua) 
( leg2 aqua) 
(leg3 aqua) 
(leg4 aqua) 
(legs aqua) 
(legs aqua) 



(first increment-list)) 
(second increment-list)) 
(third increment-list)) 
(fourth increment-list)) 
(fifth increment-list)) 
(sixth increment-list)) 
(seventh increment-list))) 



(defconstant null-move-list '((000000) (000) (000) (000) 
( 000 ) ( 000 ) ( 000 ))) 



(defmethod f easible-movep 

(and ( f eas ible-movep (legl 
( f eas ible-movep (leg2 
( f eas ible-movep (leg3 
( f easible-movep (leg4 
( f easible-movep (legS 
( teas ible-movep (legS 



( 



(aqua aquarobot) allowable-sin)cage 



allowable -slippage) 
aqua) al lowable-s inicage 
aqua) al lowable-s in)iage 
aqua) al lowable-s in)cage 
aqua) al lowable-s inicage 
aqua) al lowable-s in)cage 
aqua) a 1 lowable-s in)cage 



allowable -slippage) 
allowable-slippage) 
allowable-slippage) 
allowable-slippage) 
allowable-slippage) 
allowable-slippage) ) ) 
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1 



(requir<“ :xcw) 

(cw: init ia lize-common-windows ) 

(def class camera (rigid-body) 

( ( focal- length 

:accessor focal-length 
rinitform 6) 

(previous -point 
:accessor previous -point ) 

(camera -window 
:accessor camera -window) 

(H-matr ix 

;initform (homogeneous-transform .3 -.3 0 -300 -90 -90)) 

( inverse-H-matr ix 
taccessor inverse-H-matrix 

rinitform (inverse-H (homogeneous -trans form .3 -.3 0 -300 -90 -90))) 
(enlargement -factor 
.•accessor enlargement-factor 
: initform 30) ) ) 

(defmethod create-camera-window ((camera camera)) 

(setf (camera -window camera) 

(cwrmake-window-stream rborders 5 
rleft 500 
rbottom 500 
rwidth 300 
rheight 300 
.•title 'aquarobot' 

:activate-p t ) ) ) 

(defmethod move ( (ciunera camera) azimuth elevation roll x y z) 

(setf (H-matrix camera) (homogeneous -transform azimuth elevation roll x y z) ) 
(setf (inverse-H-matrix camera) (inverse-H (H-matrix camera)))) 

(defmethod ta)ce-picture ((camera ciunera) (body rigid-body)) 

(dolist (polygon (polygon-list body)) 

(draw-polygon camera polygon (transformed-node-list body)))) 

(defmethod draw-polygon ((camera camera) polygon node-coord- list) 

(let* ((starting-node-index (first polygon)) 

( remain ing-node- indices (rest polygon)) 

(start-point-coord (nth starting-node-index node-coord- 1 ist )) ) 

( trans form-and-move-pen-to camera start-point-coord) 

(dolist (node-index remaining-node-indices) 

( transform-and-draw-to camera (nth node-index node-coord-list))) 
(transform-and-draw-to camera start -point-coord) ) ) , •closes polygon 

(defmethod trans form-and-move-pen-to ((camera camera) point-in-earth-space) 
(setf (previous-point camera) 

(compute-camera-window-coordinates camera point-in-earth-space))) 

(defmethod transform-and-draw-to ( (c^unera c^unera) point-in-earth-space) 

(let ((to-point 

(compute-camera-window-coordinates camera point-in-earth-space) ) ) 
(draw-line-in-camera-window ceunera (previous -point camera) to-point) 

(setf (previous-point camera) to-point))) 

(defmethod draw- line- in-camera-window ((camera camera) start end) 

(cw:draw-l ine (camera -window camera) 

(cw: make -posit ion :x (first start) :y (second start)) 

(cw •make-posit ion :x (first end) ;y (second end)) 

:brush-width 0)) 

(defmethod compute-camera-window-coordinates ((camera camera) 
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3 



point -in-«arth- space) 

(let* ((enlargement-factor (enlargement-factor camera)) 

(focal-length (focal-length camera)) 

(point-in-camera-space (post-multiply ( inverse-H-matrix camera) 

point-in-earth-space) ) 

(x (first point-in-caroera-space) ) ;x axis is along optical axis 
(y (second point-in-camera-space)) ;y is out right side of camera 
(z (third point- in-camera-space) ) ) ;z is out bottom of camera 



(if (>3 X focal-length) ; handles rear clipping 

(list (♦ (round (* enlargement -factor (/ (* focal-length y) x) ) ) 
150) ;to right in ceunera window 

(♦ 150 (round (* enlargement -factor (/ (• focal -length (- r) ) 
;up in camera window 



(list -1 -1) ) ) ) 



X))))) 
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load-flla«.cl 1 

(load ’camera. cl") 

(load 'link. cl’) 

(load 'rigid-body . cl • ) 

(load 'robot -kinematics . cl ’ ) 

(load 'aqua. cl') 

(load 'aqua- leg . cl ’ ) 

(load 'aqua-link. cl’ ) 
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rigid-body. el 



(dofclass rigid-body 
() 

{ ( location 

rinitarg : location 
laccessor location) 

(va locity 

tinitarg svalocity 
:acc«ssor velocity) 

(acceleration 
:acces8or acceleration) 

( forces-and-torques 
:accessor forces -and-torques ) 

(moment s -of - inert ia 
:initarg :moments-of-inertia 
raccessor moments-of-inertia) 

(mass 

:initarg :mass 
raccessor mass) 

(node- list 
rinitarg mode-list 
raccessor node-list) 

(polygon-list 
rinitarg .‘polygon -list 
raccessor polygon-list) 

(transformed-node-list 
raccessor transformed-node-list) 

(H-matrix 

raccessor H-matrix) 

(current-time 
raccessor current-time))) 

(defmethiod move ((body rigid-body) azimuthi elevation roll x y z) 
(setf (H-matrix body) 

Oiomogeneous -transform azimutli elevation roll x y z)) 
(transform-node-list body) 

(update-position body)) 

(defmethod move -incremental ((body rigid-)x>dy) increment-list) 
(setf (H-matrix )sody) 

(matr ix-multiply (H-matrix body) (homogeneous-transform 

(first increment-list) 
(second increment-list) 
(third increment-list) 
(fourth increment-list) 
(fifth increment-list) 
(sixth increment-list)))) 

(transform-node- list body) 

(update-position Ixjdy) ) 



(defmethod get-delta-t ( (loody rigid-l)ody) ) 

(let* ( (new- time (get-internal-real-time)) 

(delta-t (/ (- new-time (current-time body)) 1000))) 
(setf (current-time body) new-time) 
delta-t) ) 

(defmethod start-timer ( ()x>dy rigid-body)) 

(setf (current-time )x>dy) (get-internal-real-time))) 

(defmethod update- rigid-lsody ( ()sody rigid-lx>dy ) ) 

(let ((delta-t (get-delta-t body))) 

(update-acceleration body) 

(update -H-matrix body delta-t) 

(transform-node- list Isody) 
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2 



(update-position body) 
(update-velocity body delta-t))) 



(del!method update-acceleration ((body rigid-ljody ) ) 

(setf (acceleration )x3dy) ;;(list u-dot v-dot w-dot p-dot q-dot r-dot) 
(multiple -value -bind 

(Fx FyFzLMN uvwpqr Ixly Iz) 

(values-list 

(append 

( forces-and-torques body) (velocity Ixjdy) (moments-of -inertia laody))) 



(list (4. 



r) (* -1 w q) (/ Fx (mass laody)) 

•gravity* (first (third (H-matrix Ixsdy) ) ) ) ) 
w p) (* -1 u r) (/ Fy (mass body)) 

•gravity* (second (third (H-matrix body))))) 
u q) (* -1 V p) (/ Fz (mass body)) 

♦gravity* (third (third (H-matrix laody))))) 
(* (- ly Iz) q r) L) Ix) 

(* (- Iz Ix) r p) M) ly) 

(* (- Ix ly) p q) N) Iz))))) 



(defmethod update- velocity ((Ixsdy rigid-body) delta-t) 

(setf (velocity Ixady) 

(vector-add (velocity body) 

( scalar-multiply delta-t (acceleration Ixjdy))))) 



(defmethod update-H-matrix ((body rigid-)xsdy) delta-t) 

(setf (H-matrix body) 

(matrix-mult iply 

(H-matrix body) (homogeneous -transform 

(* delta-t (sixth (velocity body))) 

(* delta-t (fifth (velocity body))) 

(* delta-t (fourth (velocity body))) 

(* delta-t (first (velocity iDody) ) ) 

(* delta-t (second (velocity l>ody) ) ) 

(* delta-t (third (velocity body))))))) 



(defmethod transform-node-list ((body rigid-body)) 

(setf (transformed-node-list Ijody) 

(mapcar #’ (lambda (node -location) 

(post-multiply (H-matrix body) node- locat ion ) ) 
(node-list body)))) 



(defmethod update-position ((Isody rigid-body)) 

(setf (location txady) (near 3 (first (transformed-node-list body))))) 



(defmethod get -node -polygon- list ((body rigid-lx)dy ) ) 

(list (transformed-node-list Isody) (polygon-list t>ody) ) ) 

(defconstant *gravity* 32.2185) 
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(defun transpose (A) 

(cond ((null (cdr A)) (tnapcar 'list (car A))) 

(t (mapcar 'cons (car A) (transpose (cdr A)))))) 

(defun dot -product (x y) ;A vector is a list of numerical atoms. 

(apply '♦ (mapcar '* x y))) ;A matrix is a list of row lists, 

(defun vector-length (x) (sqrt (dot-product x x) ) ) 

(defun post-multiply (M x) 

(cond ((null (cdr M) ) (list (dot-product (car M) x) ) ) 

(t (cons (dot-product (car M) x) (post-multiply (cdr M) x) ) ) ) ) 

(defun pre-multiply (x M) 

(post-multiply (transpose M) x) ) 

(defun matrix-multiply (A B) 

(cond ((null (cdr A)) (list (pre-multiply (car A) B))) 

(t (cons (pre-multiply (car A) B) (matrix-multiply (cdr A) B) ) ) ) ) 

(defun chain-multiply (L) 

(cond ((null (cddr D) (matrix-multiply (eval (car L)) (eval (cadr L)))) 

(t (matrix-multiply (eval (car D) (chain-multiply (cdr L)))))) 

(defun cycle-left (L) (mapcar ' row-cycle- left L) ) 

(defun row-cycle-left (R) (append (cdr R) (list (car R)))) 

(defun cycle-up (M) (append (cdr M) (list (car M) ) ) ) 

(defun unit-vector (one-column length) 

(do ( (n length (1- n) ) 

(R nil (cons (cond ((= one-column n) 1) (t 0)) R) ) ) 

( (zerop n) R) ) ) 

(defun unit-matrix (n) 

(do ((row-number n (1- row-number) ) 

(I nil (cons (unit-vector row-number n) I))) 

((zerop row-number) I))) 

(defun concat-matrix (A B) 

(cond ( (null A) B) 

(t (cons (append (car A) (car B) ) (concat-matrix (cdr A) (cdr B)))))) 

(defun augment (A) (concat-matrix A (unit-matrix (length A)))) 

(defun normalize-row (R) (scalar-multiply (/ 1.0 (car R)) R) ) 

(defun scalar-multiply (a x) 

(cond ((null x) nil) 

(t (cons (• a (car x) ) (scalar-multiply a (cdr x) ) ) ) ) ) 

(defun solve-f irst-column (M) 

(do* ( (LI M (cdr LI) ) 

(L2 (normalize-row (car M) ) ) 

(L3 (list L2) (cons (vector-add (car LI) 

(scalar-multiply (- (caar LI)) L2)) L3 ) ) ) 

((null (cdr LD) (reverse L3)))) 

(defun vector-add (x y) (mapcar x y) ) 

(defun vector-subtract (x y) (mapcar '- x y)) 

(defun square-car (M) 
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(do ( (tn ( length M) ) 

(LI M (cdr LI) ) 

(L2 nil (cons (near m (car LD) L2))) 

((null LI) (reverse L2)))) 

(defun nedr (n L) (cond ((zerop n) L) (t (cdr (nedr (1- n) L) ) ) ) ) 

(defun near (n L) (cond ((zerop n) nil) 

(t (cons (car L) (near (1- n) (cdr L) ) ) ) ) ) 

(defun ntnax-car- first (n L) 

(append (max-car- first (near n L) ) (nedr n L) ) ) 

(defun matrix- inverse (M) 

(do ((Ml (max-car-first (augment M) ) 

(cond ((null Ml) nil) 

(t (nmax-car-f irst n (cycle-left (cycle-up Ml ))))) ) 

(n (1- (length M) ) (1- n) ) ) 

((or (minusp n) (null Ml)) (cond ((null Ml) nil) (t (square-car Ml)))) 
(setq Ml (cond ((zerop (caar Ml)) nil) (t (solve-f irst-column Ml)))))) 

(defun max-car-first (L) 

(cond ((null (cdr L) ) L) 

(t (if (> (abs (caar L) ) (abs (caar (max-car-first (cdr L) ) ) ) ) L 
(append (max-car-first (cdr L) ) (list (car L) ) ) ) ) ) ) 

(defun dh-matrix (cosrotate sinrotate costwist sintwist length translate) 
(list (list cosrotate (- (* costwist sinrotate)) 

(* sintwist sinrotate) (* length cosrotate)) 

(list sinrotate (* costwist cosrotate) 

(- {* sintwist cosrotate)) (* length sinrotate)) 

(list 0. sintwist costwist translate) (list 0. 0. 0. 1.))) 

(defun homogeneous -transform (azimuth elevation roll x y z) 

(rotation-and-translation (sin azimuth) (cos azimuth) (sin elevation) 

(cos elevation) (sin roll) (cos roll) x y z)) 

(defun rotation-and-translation (spsi cpsi sth cth sphi ephi x y z) 

(list (list (* cpsi cth) (- (* cpsi sth sphi) (* spsi ephi) ) 

(♦ (* cpsi sth ephi) (* spsi sphi)) x) 

(list (* spsi cth) (♦ (* cpsi ephi) (• spsi sth sphi)) 

(- (* spsi sth ephi) (* cpsi sphi)) y) 

(list (- sth) (* cth sphi) (* cth ephi) z) 

(list 0. 0. 0. 1 . ) ) ) 

(defun inverse-H (H) 

(let* ((minus-P (list (- (fourth (first H) ) ) 

(- (fourth (second H) ) ) 

(- (fourth (third H) ) ) ) ) 

(inverse-R (transpose (square-car (reverse (rest (reverse H) ) ) ) ) ) 
(inverse-P (post-multiply inverse-R minus-P) ) ) 

(append (concat-matr ix inverse-R (transpose (list inverse-P))) 

(list (list 0 0 0 1) ) ) ) ) 
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APPENDIX B - C++ CODE 



bot .h 1 

1 1 ••«••••.••*•**••••*•**•**«•***•*•••••*•*•*•**•*•*•**•«** 

// FILENAME: bot . h 

// PURPOSE: defines constants and functions used Int bot.C 
// NOTE: This is an IRIS 3D program written in C++ 

// AUTHOR: S L Davidson 
// DATE: 5 January 1993 

// *•*•*•*•*•«•*•••••*•«•*•***••**••*•*•*•••••«••*•***•***•, 

// provides constants for menu processing options 

•define CAMERA 1 

•define ABOVE 2 

•define BEHIND 3 

•define RTSIDE 4 

•define LTSIDE 5 

•define FILEREAD 6 

•define KEYBDREAD 1 

•define RESETFILE 8 

•define RESET 14 

•define EXIT 15 



• define NEARCLIPPING 10.0 // planes defined 

•define FARCLIPPING 1023.0 

•define VIEWX - 0.0 /* location of the viewpoint */ 

•define VIEWY - 40.0 
•define VIEWZ - 400.0 



•define 


REFX - 


0.0 /* location of the robot */ 


•define 


REFY - 


0.0 


•define 


REF2 - 


-200.0 



long makethemenus 0 j 


















static 


float 


rfx; /* 


reference 


point 


on 


in 


the 


X 


direction 


*/ 


static 


f loat 


rfy; /* 


reference 


point 


on 


in 


the 


y 


direction 


*/ 


static 


float 


rfz;/* 


reference 


point 


on 


in 


the 


z 


direction 


*/ 



static float vx; /* view point on in the x direction */ 

static float vy; /• view point on in the y direction •/ 

static float vz; /* view point on in the z direction •/ 

double deltal, delta2, delta3; 

double delaz, dele 1, del rol, del X, dely, delz; 

void processmenuhit (long hititem) ; 

void initialize () ; // initializes graphics layout 

void loadunit () ; // a unit matrix used in rotation/translation 

void pro jectionandviewingmatrix (float vx, float vy, float vz, float ref x, float refy, float ref 
z) ; 

void buildnonmovingviewingmatrix (float vx, float vy, float vz, float refx, float refy, float re 
fr) ; 

void drawaqua (double* , double*, double *, double *, double *, 
double *, double *); // includes all legs and body 
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bot.C 



n 
n 
n 
n 
// 
// 
n 
// 
n 
1 1 
n 



FILENAME; bot.C 

PURPOSE: This file makes a stick aquarobot graphics 
interactive design 

It utilises Kinematic functions to determine xyt 
coordinates 

CONTAINS: functions shown in bot.h 

NOTE: This is and IRIS 3D program written in C-*--*- 

AUTHOR: S L Davidson 

DATE: 15 February 1993 



linclude "gl.h" ft graphics library 
finclude "device. h" II graphics library file 
linclude "bot.h" // declaration file 
finclude <stdio.h> // C++ library 
finclude "Link.H" 
finclude "RigidBody .H" 
finclude "MatrixMy.H" 
finclude "AquarobotBody . H" 
finclude "Kinematics .C" 
finclude "AquaLeg.H" 

main () 

( 



II value returned from the event queue 
short value; 
long mainmenu; 
long hititem; 

FILE *ifp; 

ifp - fop>en ("bot .dat", "r") ; 

II initialize the IRIS system 
initialize 0 ; 

// Create Pop Up Menus 
mainmenu - makethemenus ( ) ; 

II makes the robot from its pieces 
AquarobotBody aquabody; 

AquaLeg legl (aquabody, 0 . 0) ; 

AquaLeg leg2 (aquabody, 60 . 0) ; 

AquaLeg leg3 (aquabody, 120 . 0) ; 

AquaLeg leg4 (aquabody, 160 . 0) ; 

Aqual,eg legS (aquabody, 240 . 0) ; 

AquaLeg leg6 (aquabody, 300 . 0) ; 

Return_Coordinates coord; 
Passing_ltems pass; 

Next_Motion trans; 
int ans ; 
ans - 0; 

pass.legnum - 9; 



coord 

trans 



FindPos it ions (aquabody , legl, leg2, leg3, leg4, legS, leg€) ; 
TransferToGait (coord, aquabody) ; 
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2 



I 



// do we have eomething on the event queue? 

If <qtest 0 ) 

( 

switch (qread ((value) ) 

I 

case REDRAH: 

reshapeviewport () ; 
break; 

case MENUBUTTON: 

if (value — 1) 

[ 

hitltem * dopup (maininenu) ; 
processrnenuhit (hitltem) ; 

) 

break; 

// queue used for calling the gait algorithm 
case AKEY: 

trans * Gait Algorithm(trans) ; 

delaz - trans .bodymotion 1 0) ; 
delel - trans .bodymotion (1] ; 
delrol * trans .bodymotion [2] ; 
delx ■ trans.bodymotion [3] ; 
dely - trans .bodymotion ( 4) ; 
dels - trans.bodymotion(5]; 

aquabody.MoveInc remental (dela z, delel, del rol,de lx, dely, dels) ; 
legl .Moveincremental (aquabody, trans . leglmotion ( 0] , 
trans . leglmotion ( 1 ] , trans . leglmotion [2] ) ; 

leg2 .Moveincremental (aquabody, trans . Ieg2motion ( 0] , 
trans . leg2motion (1) , trans . leg2motion (2) ) ; 

leg3 .Moveincremental ( aquabody, trans . leg3motion (0] , 
trans . leg3motion [ 1 ) , trans . leg3motion [2] ) ; 

leg4 .Moveincremental (aquabody, trans . leg4motion (0] , 
trans .leg4motion [ 1 ) , trans . leg4motion (2) ) ; 

legs. Moveincremental (aquabody, trans . legSmotion (0) , 
trans . legSmotion (1) , trans . legSmotion [2 ] ) ; 

legs .Moveincremental (aquabody, trans . legSmotion [ 0] , 
trans . legSmotion | 1 ] , trans . legSmotion (2] ) ; 

coord - FindPositions (aquabody, legl, leg2, leg3, leg4, legs, legS) ; 

trans ■ TransferToGait (coord, aquabody) ; 

break; 

// reads incremental changes from a file 
case PKEY: 

printf("\n Reading file motionVn") ; 
pass * rile_Use (ifp, aquabody, legl, leg2, leg3, 
leg4, legs, legS) ; 

printf ("%d, %lf, %lf, %lf \n",pass .legnum, 
pass .dell , pass .del 2, pass .del 3) ; 
pass.legnum - 9; 

coord • FindPositions (aquabody, legl, leg2, leg3, 
leg4, legs, legS) ; 

trans - TransferToGait (coord, aquabody) ; 
break; 
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bot.C 



3 



case OKEY: 

rewind (Ifp) ; 
break; 

default : 

break; 

) // end switch on event queue item 

) // endlf qtest 0 



color (BLUE); // background color 
clear O ; 

// turn on Z-buffering 
zbuffer (TRUE) ; 

// clear the z-buffer 
zclear () ; 

bulldnonmovingvlewingmatrix (vx, vy, vz, rfx, rfy, rf z) ; 
drawaqua ( (coord. bodyc) , (coord . leglc) , (coord . Ieg2c) , (coord . Ieg3c) , 
(coord. Ieg4c) , (coord . legSc ) , (coord. Ieg6c) ) ; 

// turn z-buffering off 
zbuffer (FALSE) ; 

// change the buffers 
swapbuf f ers ( ) ; 

qenter(AKEY, 1) ; 

) 

) // end of main 



II 

// FUNCTION: INITIALIZE () 
// 

void initialize () 

I 



// set up the preferred aspect ratio 
keepaspect (XMAXSCREEN+1, YMAXSCREEN+1) ; 

// set up window size 

prefposition 000.0, 1200. 0,200. 0,700.0) ; 

// open a window for the program 
winopen ("Aqua Robot") ; 

// make a title 
wintitle ("AquaRobot") ; 

// put the IRIS into double buffer mode 
doublebuffer ( ) ; 
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// configure the IRIS (means use the above command settings) 
gconf ig () ; 

// define acceptable queues 

qdevice (REDRAW) ; 

qdevice (AKEY) ; 

qdevice (PREY) ; 

qdevice (RKEY) ; 

qdevice (MENUBUTTON) ; 

// initial location of viewpoint (camera eye) 
vx - 0.0; 
vy - 40.0; 
vz - 400.0; 

// initial location of robot foot pads 
rfx - 0.0; 
rfy - 0.0; 
rfz - -200.0; 



) 



// 

// Function Ma)ce_the_Menus 

H ••**.*.•••*.*.**..**.•• 

long makethemenus 0 
( 



long topmenu; 
long cameramenu; 

if camera views 
cameramenu - newpupO; 

addtopup (cameramenu, “Camera View %t ") ; 
addtopup (cameramenu, "ABOVE %x2 ( BEHIND 4x3"); 
addtopup (cameramenu, "RIGHT SIDE 4x4 | LEFT SIDE 4x5"); 



// build the top level menu 

topmenu- defpup("Roll Off Side 4t I Camera 4x1 4m| 

FileRead 4x6 IResetFile 4xB I KeybdRead 4x7 | 
Reset 4x141 Exit 4x15" , cameramenu) ; 

// return the name of this menu 
return (topmenu) ; 



// Function Process_Menu Hit 

void processmenuhit (long hititem) 
( 

switch (hititem) 

I 

case CAMERA: 

break; 
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case ABOVE: 



vx 


- 0.0 


vy 


- 300.0 


vz 


- 0.0 


rfx 


- 0.0 


rfy 


- 0.0 


rfz 


0.0 


break; 



case BEHIND: 



vx - 0.0; 
vy • 50.0; 
vz - 250.0; 
rfx - 0.0; 
rfy - 30.0; 
rfz - -200.0; 
break; 



case RTSIDE: 

vx - 250.0; 
vy - 50.0; 
vz - 0.0; 
rfx - -200.0 
rfy - 50.0; 
rfz - 0.0; 
break; 



case LTSIDE: 

vx - -250.0; 
vy - 30.0; 
vz - 0.0; 

rfx - 0.0; 

rfy - 0.0; 

rfz - -200.0; 
break; 



case FILEREAD: 

qenter (PREY, 1) 
break: 

case RESETFILE: 

qenter (RKEY, 1» 
break; 

case RESET: 

vx -0.0; 
vy -40.0; 
vz - 400.0; 
rfx - 0.0; 
rfy - 0.0; 
rfz - -211.0; 
break; 

case EXIT: 

exit(O) ; 
break; 

I // End Switch 
I 
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// FUNCTION: BUILDNONMOVINGVIEWINGMATRIX 

// PURPOSE: use with objects that are in the same coordinate 
// system and aren't moving with continuous 

// rotations/translations /scalings 



void buildnonmovingviewingmatrlx (float vx, float vy, float v*, 
float refx, float refy, float refz) 



( 



loadunit 0 ; 



pro jectionandviewingmatrix (vx, vy, vz, refx, refy, refz) ; 



) 



// 

// FUNCTION: PROJECTIONANDVIEWINGMATRIX 

// PURPOSE: provides the projection and viewing matrix 

// 

void pro jectionandviewingmatrix (float vx, float vy, float vz, float refx, float refy, float ref 
2 ) 

I 



// perspective projection 3D for the world coord sys 
// the near and far values are distances from the viewer 
// to the near and far clipping planes. 

// We are at (vx,vy,vz) and looking towards 
// the center point of the object.. 

// (towards (ref x, ref y, ref z) ) . 

perspective (450, 1.25, NEARCLIPPING, FARCLIPPING) ; 
lookat (vx, vy, vz, refx, refy, refz, 0) ; 



) 

// 

// FUNCTION: LOADUNIT 

// PURPOSE: this routine loads a unit matrix onto the top 

// of the stack 



void loadunit 0 
( 

static float un(4)MJ - ( 1-0, 0.0, 0.0, 



0 . 0 , 1 . 0 , 0 . 0 , 0 . 0 , 

0 . 0 , 0 . 0 , 1 . 0 , 0 . 0 , 

0 . 0 , 0 . 0 , 0 . 0 , 1.0 ); 



loadmatrix (un) ; 



) 

// *•***.**•.**«•«••***..•.*•***••«.**••****•**«••••••• 

// FUNCTION: AQUA DRAWING 

// PURPOSE: draws the robot at coordinates provided 

// 

void drawaqua (double *bodyc, double *leglc, double *leg2c, double 
*leg3c, double *leg4c, double *leg5c, double *leg6c) 

{ 

color (WHITE) ; 
linewidth (3) ; 
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// +x to right, +y up, ♦t out of screen ->for graphics 
// +x out of legl, +y out of screen, -z down->for kinematics 



// X z y 

move (bodyc [3) , -bodyc 15] ,bodyc [4] ) ; 
draw (bodyc ( 6] , -bodyc [8] , bodyc (7] ) ; 
draw (bodyc | 9) , -bodyc (11] , bodyc ( 10] ) ; 
draw (bodyc [ 12] , -bodyc (14], bodyc ( 13] ) ; 
draw (bodyc ( 15] , -bodyc ( 17] , bodyc (16]) ; 
draw (bodyc ( 18 ] , -bodyc (20] , bodyc (1 9] ) ; 
draw (bodyc (3] , -bodyc (5] , bodyc ( 4] ) ; 

// draws a line from body center to legl joint 1 
linewidth(l) ; 

move (bodyc ( 0 ] , -bodyc ( 2 ] , bodyc ( 1 ] ) / 
draw (bodyc ( 3 ] , -bodyc ( 5 ] , bodyc ( 4 ] ) ; 

// draws legl 
color (YELLOW) ; 
linewidth (5) ; 

// X z y 

move (leglc (0] , -leglc (2] , leglc (1 ] ) ; 
draw(leglc(3],-leglc(5],leglc(4]) ; 
draw (leglc (6] , -leglc (8) , leglc (7] ) ; 
draw (leglc ( 9] , -leglc (11] , leglc ( 10] ) ; 

// draws leg2 
color (GREEN) ; 
linewidth (5) ; 

move ( leg2c ( 0] , -leg2c ( 2] , leg2c (1]); 
draw(leg2c (3] , -leg2c (5] , leg2c (4] ) ; 
draw (leg2c ( 6] , -leg2c ( 8] , leg2c (7 ] ) ; 
draw (leg2c ( 9] , -leg2c (11 ] , leg2c (10] ) ; 

// draws leg3 
color (GREEN) ; 
linewidth (5) ; 

move (leg3c (0] , -leg3c (2 ] , leg3c ( 1] ) ; 
draw(leg3c(3],-leg3c(5],leg3c(4]); 
draw(leg3c(6],-leg3c(8],leg3c(7]); 
draw(leg3c (9] , -leg3c(ll] , leg3c(10] ) ; 

// draws leg4 
color (GREEN) ; 
linewidth (5) ; 

move (leg4c (0] , -leg4c (2] , leg4c (1] ) ; 
drawdeg4c(3],-leg4c(5],leg4c(4]); 
draw(leg4c(6],-leg4c(8],leg4c(7]),- 
draw(leg4c(9],-leg4c(ll] , leg4c(10]) ; 

// draws leg5 
color (GREEN) ; 
linewidth (5) ; 

move (leg5c (0] , -leg5c (2] , leg5c (1] ) ; 
draw(leg5c (3] , -leg5c(5] , leg5c (4] ) ; 
draw(leg5c(6],-leg5c(8],leg5c(7])/ 
draw(leg5c(9],-leg5c(ll],leg5c(10]) ; 

// draws leg6 
color (GREEN) ; 
linewidth (5) ; 

move (leg 6c (0] , -leg6c (2] , leg6c(l]) ; 
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draw (leg6c [3] , -legSc [5] , leg6c [4 J ) ; 
draw (leg6c [ 6] , -leg6c [8] , legSc f7] ) ; 
draw (ieg€c( 9] , -leg6c[ll) , leg6c [10] ) 
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// FILENAME: AqusL«g.H 

// PURPOSE: Declarations for AquaLeg claaa 
// 

// AUTHOR: S L Davidson 
// DATE: 17 Feb 93 

// COMMENTS: Definition of AquaLeg claas and functions that 
// apply to this claas 

// «•«•**•*•*•****•«««*«***•*«««**«««««*««**««««**«**«««*«* 

#ifndef H_AQUALEG 
« define H_AQUALEG 

linclude <stdio.h> 
finclude "AquarobotBody .H" 

•include -Link.H* 

•include "LlnkO.H" 

•include "Linkl.H* 

• Include *Link2.H*' 

•include "LlnkS.H" 

class AquaLeg 

{ 

public : 

// these dependent objects are Instantiated 
LinkO *linkO; 

Linkl *linkl; 

Link2 *link2; 

Link3 *llnk3; 

// the flag is set to 1 if the motion la completed without 
// reaching any link limita 
Int motlon_complete_f lag; 

// the flag is set to 1 if the leg is on the ground 
int leg_support_flag; 

// the angle off of leg one where the leg is attachec to 
// the body 

double leg_attachment_angle; 



AquaLeg (AquarobotBodyK, double); // constructor and initializer 
~AquaLeg(|; // destructor 

void Moveincremental (AquarobotBody fc, double deltsl, double delta2, 
double delta3) ; 

double GetLegAttachmentAngle (} ( return leg_attachment_angle; I 

int GetMotionCompleteFlag 0 ( return motion_complete_f lag; | 

void SetLegAttachmentAngle (double angle) ( leg_attschment_angle “ angle 

void SetMotionCompleteFlag (int flag) (motlon_complete_f lag - flag;) 

int GetLegSupportFlag 0 ( return leg_support_flag; ) 

void SetLegSupportFlag (int flag) ( leg_support_f lag - flag;) 
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// *•****•**••****•*••••***•*••*****•••*••* 

// FILENAME: AquaLeg.C 

// PURPOSE: Implementation of AquaLeg class 



// CONTAINS 


; AquaLeg 0 




// 


Initialize (AquaLegt, 


AquarobotBodyt ) 


// 


TakePicture (Camerat , 


AquaLegC ) 


// 


Move Inc remental (Aqualegt, deltal,delta2,delta3) 


// AUTHOR: 


S L Davidson 





// DATE: 17 Feb 93 

tinclude "AquaLeg. H" 

// ****.**.*.*.*.*************.***, 

// FUNCTION; ‘AquaLeg () 

// PURPOSE; destructor of AquaLeg class 

// ************** .**.*.*.*.... 

AquaLeg ; ; ‘AquaLeg ( ) 

\ 

delete linkO; 
delete linkl; 
delete llnk2; 
delete link3; 

) 



// FILENAME: AquaLeg 

1 1 PURPOSE: constructor of AquaLeg class 

It RETURNS: AquaLeg class with values 

H *•***«*«••*****•*«*«*•*****«*«*«*••«***«*•*••••*••*•«* 

AquaLeg: ‘.AquaLeg (AquarobotBody &body, double angle) 

( 

motion_complete_f lag - 1; // initialires flag value 

SetLegAttachmentAngle (angle) ; 

linkO - new LinkO; 

linkl • new Linkl; 

link2 - new Link2; 

links w new Link3; 



// initial link values initialized 

// temp matrix adds in the T_matrix needed for the physical 
// attachment of the leg to the body 
matrix temp; 

// updates the Transformation matrix from body center to the 
// leg attachment point 

temp.UpdateTMatrix (GetLegAtt achment Angle () , 0 . , 0 . , 0 . ) ; 
temp - *body . H_matrix * temp; 

linkO->RotateLink (Ctemp , linkO->Get InboardJointAngle ( ) ) ; 

linkl->RotateLink (linkO->H_matrix, linkl->GetInboardJoint Angle () ) ; 
link2->RotateLink (linkl->H_matrix, link2->OtInboardJointAngle {) ) ; 
link3->RotateLink (link2->H_matrix, link3->Get InboardJointAngle {) ) ; 

) 

H «««*«««*«***••*****••*«*•««***••***•****«**««••«•«•*•***** 

// FILENAME: Move Increment a 1 

// PURPOSE: calculate the new link values as a leg rotates 
// RETURNS: rotated link's new values are placed in the 

// respective leg's slots 

H * 
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void AquaL«g: :Movelncrem«ntal(AquarobotBody (bodyrdouble d«ltal, 
double delta2, double deltaS) 



double b; 

// set all limit flags to *ero 
Hn1cl->SetMotionLimitriag(0) ; 
link2->SetMotionLimitFlag(0) ; 
link3->SetMotionLimltFlag (0) ; 

// temp matrix adds in the T_matrix needed for the physical 
// attachment of the leg to the body 
matrix temp; 

temp.UpdateTMatrix (GetLegAttachmentAngle () , 0 . , 0 . , 0 . ) ; 
temp - *body.H_^matrix * temp; 

linkO->RotateLink (Ctemp, linkO->GetInboardJointAngle () ) ; 

b •• deltal + linkl->GetlnboardJointAngle 0 ; 
linkl->SetInboardJointAngle (b) ; 

linkl->RotateLink (linkO->H_matrix, linkl->GetlnboardJointAngle () ) 

b •• delta2 + link2->GetInboardJointAngle 0 ; 
link2->SetInboardJointAngle (b) ; 

link2->Rotate (linkl->H_^matrix, Hnk2->GetInboardJointAngle () ) ; 

b - delta3 + link3->GetInboardJointAngle () ; 
link3->SetInboardJointAngle (b) ; 

link3->RotateLink(link2->H_matrix, link3->GetInboardJointAngle () ) 

// the motion_complete_flag is set to 1 if the 
// motion_limit_f lags on all legs are not set 
SetMotionCompleteFlag ( ! (linkl->GetMotionLimitFlag() I I 

link2->GetMotionLimitFlag() || link3->GetMotionLimitFlag ( ) ) ) ; 

// prints the status of the requested motion and prints which 
// link's motion_limit_f lag was set (if any), 
if (GetMotionCompleteFlag 0 — 0) 

(printf ("Motion Not Completed\n") ; 
if (linkl->GetMotionLimitFlag() — 1) 
printf ("link 1 limit exceeded\n") ; 
if (link2->GetMotionLimitFlag() 1) 
printf ("link 2 limit exceeded\n") ; 
if (link3->GetMotionLimitFlag() — 1) 
printf ("link 3 limit exceeded\n") ; 

) 

else printf ("Motion completedNn") ; 



) 
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1 



// * *.*.•.***.*..* 

// FILENAME; Aqua robotBody .K 

// PURPOSE: Declaration of AquarobotBody class 
// Subclass of Rlqldfiody class 

// AUTHOR: S L Davidson 
// DATE: 20 Sep 92 

1 1 •*••••****••*•*•****•**•*•*****•*•**•*•«***«***•*•**«***•*** 

#ifndef H_AQUAROBOTBODY 
•define H_AQUAROBOTBODY 

•include <stdio.h> 

•include "RigidBody .H" 

•include "MatrixMy.H" 

class AquarobotBody : public RigidBody 
( 

public: 

matrix *body^list; // defines the size of the body using coordinates 
double azimuth, elevation, roll; 

Aqua robotBody () : // constructor 

void Move Incremental (double, double, double, double, double, double); 



•endif 
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// *«••«••*•*•«••**«*••«•*««***•*«**.*•«••*•**«****.* 

// FILENAME: AquarobotBody .C 

// PURPOSE: Implementation of the AquarobotBody class 
// CONTAINS: initialites the body form 
// AUTHOR: S L Davidson 

// DATE: 17 Nov 92 

// 

finclude "AquarobotBody .H" 



// FUNCTION: AquarobotBody () 

// PURPOSE: constructor of the AquarobotBody class 
// RETURNS: AquarobotBody class with values 



AquarobotBody: :AquarobotBody () 

{ 

1 1 each row is a body point (x,y,z) 

// the firat (0 row) ia the body' a physical center, and the rest 
// are the six points of the body 
body_list - new matrix <7, 4 , 0 . 0) ; 



// the body's coordinates are defined centered at 0,0,0 
body_liat->val (0, 0) - 0.; body_liat->val (0, 1) - 0.; 
body_list->val (0, 2) - 0.; body_list->vsl <0, 3) - 1.? 
body_list->val (1, 0) - 37.5; body_list->vsl (1, 1) « 0 . ? 
body_liat->val(l,2) - 0.; body_list->val (1, 3) - 1.; 
body_list->val <2, 0) - 18.75; body_list->val (2, 1) - 32.48; 
body_list->vsl (2, 2) - 0.; body_liat->val (2, 3) - 1.; 
body_liat->val(3,0) - -18.75; body_liat->val (3, 1) - 32.48; 
body_liat->val (3, 2) - 0.; body_list->val (3, 3) - 1.; 
body_liat->val(4,0) - -37.5; body_list->val (4, 1) - 0.; 
body_list->val (4, 2) - 0.; body_list->val (4, 3) - 1.; 
body_list->val(5,0) - -18.75; body_list->vsl (5, 1) - -32.48; 
body_liat->val(5,2) - 0.; body_list->val (5, 3) - 1.; 
body_list->val(6,0) - 18.75; body_liat->val (6, 1) - -32.48; 
body_list->val (6, 2) ■ 0.; body_list->val (6, 3) ■ 1.; 



// defines the initial location of the body using the 
// H-matrix the inputs to the function are: 

// (azimuth, elevation, roll, x, y, z) 

H_matrix->HomogeneousTrsnsf orm(0 . ,0.,0.,0.,0., -54.1819); 



// moves the body coordinates to the initial location desired 
body_list->TransformList (*H_matrix, *body_list) ; 



1 



// FUNCTION: Move Increment si 

// PURPOSE: the body is moved based upon commanded incremental 
// degrees of change that are passed in 

// •*•*.****•••«**•****•*•*•**•*«**•..*•*«*•*«*••«•«*«•«•«•«•* 



void AquarobotBody : :Movelncremental (double delaz, double delel, 
double delrol, double delx, double dely, double delz) 

( 

double az, el, ro, x, y, z; 
az - azimuth + delaz; 
el - elevation -f delel; 
ro - roll + delrol; 

X - body_list->val(0, 0) 4 delx; 
y - body_list->val(0, 1) 4 dely; 
z ■ body_list->val (0,2) 4 delz; 
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// only changes are used below since body_list Is at current position 
H_matrix->Homo 9 eneousTransfonn(delaz, delel, delrol, delx, dely, delz) ; 
body_list->Trans£onnLlst {*H_matrix, *body_list) ; 

// puts all Info in H_matrix 
H_matrix->Hofnogeneou8Transform(az, el, ro, x, y, z) ; 
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1 1 ••••••••••*••••*••*•••***•***•**«*•••*••*•••••*•••••*•• 

// FILENAME: Kinematica.C 

1 1 PURPOSE: to determine positions (x, y, z) from the H_matrlx 
// : to read from a file the new link angle changes 

// and update the appropiate leg/link values 

// : to pass items to the gait function 

// AUTHOR: S L Davidson 
// DATE: IS February 1993 

// •••*•*•****•«******.**«•«**«**•**••*•*******«•****•***• 

f include <math.h> 
tinclude <stdlib.h> 
finclude <stdio.h> 
f include "MatrixMy.H" 
finclude "AquaLeg.H" 
finclude "AquarobotBody .H" 
finclude "Link.H" 

f define GROUNDELEVATION 0.0 



// structure designed to receive file input 
struct Passing_ltems { 
int legnum; 
int body; 
double dell; 
double del2; 
double del3; 
double del 4: 
double delS; 
double del€; 

); 



// structure recieves next desired robot motion from gait 
// planning functions 
struct Next_Motion { 



// desired joint increment values returned from the gait function 
double bodymotion ( 6] ; 
double leglmotion (3] ; 
double leg2motion [3} ; 
double leg3motion (3] ; 
double legfmotion [3] ; 
double legSmotion (3] ; 
double legSmotion [3] ; 



// 



actual position status values sent to gait function 

double leg_contact_f lagi 6J ; 

double joint_limit_f lagllS) ; 

double foot_l_coordl3] ; 

double foot_2_coordl3] ; 

double foot_3_coord 13] ; 

double f oot_4_coord(3) ; 

double f oot_5_coord t 3) ; 

double f oot_6_coord 13); 

double body_center_coord [6] ; 



// structure designed to consolidate the xyr coordinates of the robot 
struct Return__Coordinates < 
double bodyc(2l); 
double leglc(l2); 
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double Ieg2c(l2]; 
double leg3c(12]; 
double leg4c(12]; 
double leg5c(121; 
double leg6c|12]; 
int motion_limlt_£lag(18) ; 
int leg_8upport_flag 1 6) ; 



// xyz coordinates are determined from the H_matrix and 

// return the Cartesian coordinates using the Return_Coordinates structure 
Return_Coordinates FindPositions (AquarobotBody (body, AquaLeg ftlegl, 
AquaLeg ftleg2, AquaLeg &leg3, AquaLeg 41eg4, AquaLeg ftlegS, 

AquaLeg 4leg6) 



i 



Return_Coordinates *rc; 
rc - new Return_Coordinates ; 

// body center coordinates 
rc“>bodyc(0] ■ body ,body_list->val (0, 0) 
rC“>bodyc(l) ■ body .body_list->val <0, 1) 
rc->bodycl2) - body .body_liat->val (0, 2) 
// body points to draw 
rc->bodyc(3] - body .body_list->va 1(1, 0) ; 
rc->bodycI4] - body .body_list->val (1, 1) ; 
rc->bodyc(5] • body .body_list->val (1, 2) ; 
rc->bodyc(6) - body .body_list->val (2, 0) ; 
rc->bodyc(7) - body .body_list->val (2, 1) ; 
rc->bodycl8) - body .body_llst->val (2, 2) ; 
rc->bodycl9] - body .body_list->val (3, 0) ; 
rc->bodyc 1 10] - body .body_list->val (3, 1) ; 
rc->bodyc 111) ■ body. body_list->val (3,2) ; 
rc->bodyc 112] - body .body_list->val (4, 0) ; 
rc->bodyc 1 13] - body .body_liat->val (4, 1) ; 
rc->bodyc 1 14 ] • body .body_liat->val (4, 2) ; 
rc->bodyc [15] - body .body_list->val (5, 0) ; 
rc->bodyc 116] - body .body_list->val (5, 1) ; 
rc->bodyc (17) - body .body_list->val (5, 2) ; 
rc->bodyc ( 18] - body .body_list->wal (6, 0) ; 
rc->bodyc 1 19] - body .body_list->val (6, 1) ; 
rc->bodyc (20) - body. body_list->va 1(6,2); 



// prints out body coordinates 

printf("body center %3f , I3f , %3f \n" , rc->bodyc (0) , rc->bodyc (1) , 
rc->bodyc(2]) ; 

printf("body pt 1 %3f, %3f, I3f \n”, rc->bodyc (3) , rc->bodyc (4) , 

rc->bodyc(5] ) ; 

printfrbody pt 2 %3f, %3£, %3f \n", rc->bodyc ( 6] , rc->bodyc (7) , 
rc->bodyc ( 8] ) ; 

printf ("body pt 3 %3f, %3f, %3f \n", rc->bodyc ( 9] , rc->bodyc ( 10] , 
rc->bodyc ( 11] ) ; 

printf ("body pt 4 %3f, %3f, %3f \n", rc->bodyc ( 12] , rc->bodyc ( 13 ] , 
rc->bodyc ( 14 ] ) ; 

printf ("body pt 5 %3f, %3f, %3f \n", rc->bodyc ( 15] , rc->bodyc ( 16] , 
rc->bodyc (17) ) ; 

printf ("body pt 6 %3f, %3f, I3f \n\n", rc->bodyc ( 18] , rc->bodyc ( 19] , 
rc->bodyc (20) ) ; 



// joint one leg coordinates: (OJ-x (l]-y (2) 
rc->leglc(0] - legl . lin)cO->H_matrix->val (0, 3) 
rc->leg2c(0] ■ leg2 . lin)c0->H_mat rix->val (0, 3) 
rc->leg3c(0] ■ leg3 . lin)cO->H_matrix->val (0, 3) 
rc->leg4c(0] - leg4 . lin)cO“>H_matrix->val (0, 3) 
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rc->leg5c[0) 
rc->leg6c [0] 
rc“>leglc 1 1] 
rc->leg2c [1) 
rc->leg3c 1 1 ) 
rc->leg4c [1] 
rc->leg5c [ 1 ) 
rc->leg6cll) 



l«g5 . linkO->H_matrix->val (0, 3) ; 
legs. linJcO->H_matrix->val (0, 3) ; 
legl . linkO->H_matrix->val (1,3); 
leg2 . linkO->H_matrix->val (1, 3) ; 
leg3 . linkO“>H_matrix->val (1,3); 
leg4.1inkO->H_matrix“>val(l,3) ; 
legs. llnkO->H_matrix->val (1,3) ; 
legs . llnkO“>H^matrix“>val (1,3) ; 



rc->leglc (2) 
rc->leg2c (2) 
rc->leg3c [2] 
rc->leg4c (2) 
rc->legSc[2] 
rc->legSc (2] 



legl . llnkO~>H_matrix->val (2,3) ; 
leg2 . linkl->H_matrix->val (2,3); 
leg3 . linkl->H_matrix->val (2,3); 
leg4 ,linkl->H_matrix->val (2,3) ; 
leg5.1inkl->H_matrix->val(2, 3) ; 
legS . linkl->H_matrix->val (2, 3) ; 



// joint 2 x,y, 
rc.->leglc (3) 
rc->leg2c[3] 
rc->leg3c [3] 
rc->leg4c(3] 
rc->legSct3] 
rc->legSc (3] 
rc->leglc [4] 
rc->leg2c [4] 
rc->leg3c[4] 
rc->leg4c [4] 
rc->legSc(4] 
rc->leg6c(4] 



* coordinates 
legl . linkl->H 
Ieg2.1inkl->H‘ 
Ieg3.1inkl->H' 
leg4 . linkl->H 
legS.linkl->H‘ 
legs . linkl->H^ 
legl . linkl->H 
Ieg2.1inkl->H“ 
Ieg3.1inkl->H" 
leg4 . linkl->H 
legS.linkl->H“ 
legS.linkl->H' 



(3)-x 

matrix' 

matrix' 

matrix' 

matrix' 

matrix' 

matrix' 

matrix' 

matrix 

matrix 

matrix 

matrix 

matrix 



[4]-y (5)-z 
->val(0,3) ; 
->val(0,3) ; 
->val(0,3); 
->val(0,3); 
->val(0,3) ; 
->val(0,3) ; 
->val(l,3); 

>wal(l,3) ; 
->val(l,3) ; 
->val(l,3) ; 
->val(l, 3) ; 
>val(l,3) ; 



rc->leglc (5] 
rc->leg2c (5] 
rc->leg3c(5] 
rc->leg4c(5) 
rc->leg5c(5) 
rc->legSc (5] 



legl . linkl->H_matrix“>val (2,3); 
leg2.1inkl->H_matrix->val(2, 3) ; 
leg3 . linkl-»(_matrix->val (2,3); 
leg4 . linkl->H_matrix->val (2, 3) ; 
legs. linkl->)(_matrix->val (2,3); 
legS.linkl->H_^matrix->val (2, 3) ; 



// joint motion_^limit_f lag 

rc->motion_limit_f lag(0] -legl . linkl-X^tMotionLimitFlag () ; 
rc->motion_limit_f lag (3] - leg2.1inkl->OtMotionLimitFlag(); 
rc->motion_limit_f lagISJ - leg3 . linkl->GetMotionLimitFlag () ; 
rc->motlon_limlt_f lag(9] - leg4.1inkl->GetMotionLimitFlag(); 
rc->motion_limit_f lag(12) - legS . linkl->GetMotionLimitFlag () 
rc->motion_limit_f lag (15) - legS . linkl->(5etMotionLimitFlag () 



// joint 3 xyz 
rc->leglc(6) - 
rc->leg2c(6) - 
rc->leg3c(S) - 
rc->leg4c|s) - 
rc->legSc(S) - 
rc->legSc(S) - 



coordinates |S]-x |7)-y [8)-z 
legl . link2->H_matrix->val (0,3); 
leg2 .link2->H_matrix->val (0, 3) ; 
leg3 . link2“>H_matrix->val (0,3) ; 
leg4 . link2->H_matrix->val (0,3) ; 
legs . link2->H^matrix->val (0, 3) ; 
legS.link2->H_matrix->val (0,3); 



rc->leglc (7) 
rc->leg2c (7 J 
rc->leg3c (7) 
rc->leg4c (7) 
rc->legSc (7) 
rc->legSc (7) 



legl . link2->H_matrix->val (1,3) ; 
leg2 . link2->H_matrlx->val (1,3) ; 
leg3 .link2->H_matrix->val (1, 3) ; 
leg4 .link2->H_matrix->val (1, 3) ; 
legs . link2->H_matrix->val (1,3) ; 
legs . link2'”>H_matrix->val (1,3) ; 



rc->leglc (8) 
rc->leg2c (8) 
rc->leg3c[8) 
rc->leg4c[8) 



legl .link2->H_matrix->val (2, 3) ; 
leg2. link2->H_matrix->val (2, 3) ; 
leg3 . link2->H_mat rix->val (2,3) ; 
leg4 . link2->H_matrix->val (2, 3) ; 
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rc->leg5c(8] - legS. link2->H_matrix->val (2, 3) ; 
rc->leg€cI8) - l«g6 . link2->H_matrix->val (2, 3) ; 

// joint 3 motion_limlt_£lag 

rc->motion_limit_£lag (iT - legl. llnk2->GetMotlonLlmltFlag<) ; 
rc->motion_limit_£lag(4) •• leg2 . Hnk2->G«tMotlonLlmitFlag <) ; 
rc->motion_limit_£lag(7) - leg3 .Hnk2->G«tMotionLiinitFlag <) ; 
rc->motion_limit_£lag(10] - l«g4 . link2->GetMotionLimltFlag () ; 
rc->motion_liinit_£lagI13) ■ leg5.1ink2->GetMotionLlmitFlag() ; 
rc->motion_limit_£lagI16) - leg6 . link2->G®tMotionLimitFlag () ; 



// joint 4 xy* coordinates I9]-x I10)-y 111]-* 
rc->leglc|9] - legl . link3->H_inatrix->val (0, 3) ; 
rc->leg2c(9) - leg2. link3->H_matrix->val (0, 3) ; 
rc->leg3cI9) - leg3 . link3->H_matrix->val (0, 3) ; 
rc->leg4cl9) - leg4 . link3->H_inatrix->val (0, 3) ; 
rc->leg5ci9) - legS. link3->H_jnatrix->val (0, 3) ; 
rc->leg6c(9) - leg6.1ink3->H_matrix->val (0, 3) ; 



rc->leglc (10) 
rc->leg2c (10) 
rc->leg3c (10) 
rc->leg4c (10) 
rc->leg5c (10) 
rc->leg6c (10) 



legl . link3->H_matrix->val (1,3) ; 
leg2.1ink3->H_matrix->val (1,3) ; 
leg3 . link3->H_matrix->val (1,3); 
leg4 . link3->H_matrix->val (1,3); 
legS . link3->H_matrix->val (1,3); 
leg6 . link3->H_matrix->val (1,3); 



rc->leglc | 11) 
rc->leg2c(ll) 
rc->leg3c (11) 
rc->leg4c(ll) 
rc->leg5c ( 11) 
rc->leg6c (11) 



legl . link3->H_matrix->val (2, 3) ; 
leg2 . link3->H_matrix->val (2,3); 
leg3 . link3->H_matrix->val (2,3); 
leg4 .link3->H_matrix->val (2, 3) ; 
legs . link3->H_n»atrix->val (2, 3) ; 
leg€ . link3->H_matrix->val (2,3) ; 



// joint 3 motion^lijnit_£lag 

rc->motion_llmit_£lag(2) - legl . link3->(;«tMotionLiinitFlag () ; 
rc->motion_limit_£lag(5) - leg2 , link3->GetMotionLimitFlag () ; 
rc->motion_limit_£lag (8) - leg3 . link3-X5etHotionLimitFlag () ; 
rc->motion_liinit_£lag (11) - leg4 . link3->GetMotionLimitFlag () ; 
rc->motion_limit_£lag(14) - legS . link3->GetMotionLimitFlag () ; 
rc->motion_limit_£lag(l7) - leg€ . link3->GetMot ionLimitFlag () ; 



// test for supporting legs and adjusting leg_support_£lag 

i£ (£abs(rc->leglc(ll)) >- GROUNDELEVATION) legl . SetLegSupportFlag (1) ; 
else legl .SetLegSupportFlag (0) ; 

i£ (fabs(rc->leg2c(ll)) >- GROUNDELEVATION) leg2 . SetLegSupportFlag (1) ; 
else leg2.SetLegSupportFlag(0) ; 

if (fabs(rc->leg3c(ll) ) >- GROUNDELEVATION) leg3 . SetLegSupportFlag (1) ; 
else leg3 . SetLegSupportFlag (0) ; 

if (fabs (rc->leg4c(ll) ) >- GROUNDELEVATION) leg4 . SetLegSupportFlag ( 1) ; 
else leg4 .SetLegSupportFlag (0) ; 

if (£abs(rc->leg5c(ll)) >- GROUNDELEVATION) legS . SetLegSupportFlag ( 1) ; 
else legs. SetLegSupportFlag (0) ; 

if (fabs(rc->leg6c(ll)) >- GROUNDELEVATION) leg6 . SetLegSupportFlag (1) ; 
else leg6.SetLegSupportFlag(0) ; 



// places leg_support_flag 
rC“>leg_support_f lag ( 0 ) 
rc->leg_support_flag (1) 
rc->leg_support_f lag (2) 
rc->leg_support_f lag (3) 
rc->leg_support_f lag (4) 
rC“>leg_support_f lag ( S] 

// prints body and leg xy* 



into rc 

- legl .GetLegSupportFlag () 

- Ieg2 .GetLegSupportFlag () 

- Ieg3 .GetLegSupportFlag () 

- Ieg4 .GetLegSupportFlag () 

- legs .GetLegSupportFlag () 

- Ieg6 .GetLegSupportFlag () 

coordinates 
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int row, col; 
printf ("logl 

for ( row - 1; row<5; row+4) 



( 



for (col - 3; col>0; 

printf ("%6.4f 
printf (" ") ; 

for (col ■ 3; col>0; 

printf (-%6.4f 
printf ("\n") ; 



col — ) 

", rc->leglc (3 
col — ) 

*•, rc->leg2c [3 



) 

printf ("\n") ; 
printf (-leg3 

for (row "I; row<5; row++) 



for (col - 3;col>0; col — ) 

printf ("%6. 4f ", rc->leg3c | 3 

printf (" ") ; 

for ( col • 3; col>0; col — ) 

printf ("%6 . 4f ", rc->log4c 1 3 

printf ("\n") ; 

1 

printf ("\n") ; 
printf ("logs 

for (row - 1; row<5; row++) 



( 



for (col ■ 3; col>0; col — ) 
printf {-%6.4f ", rc->legSc 1 3 

printf (- -); 

for (col - 3; col>0; col — ) 
printf (■%€. 4f " , rc->leg6c 1 3 

printf ("\n") ; 

) 

printf ("\n") ; 



log2\n") ; 



row - col] ) ; 



row - col] ) ; 



leg4\n") ; 



row - col] ) ; 



row - col] ) ; 



leg€\n") ; 



row - col] ) ; 



row - coll ) ; 



I 



return *rc; 



// ***.*.****.**.*******.*************, 

// FUNCTION; File_Use 

// PURPOSE: reads desired leg changes from a file 
// INPUT: reads from file: 

// format: legl, deltal, delta2, delta3 

// OUTPUT: calculates new leg/link coordinates 

// *********** * 



Passing_Items File_Use (FILE *ifp, AquarobotBody (body, AquaLeg (legl, 
AquaLeg ( leg2, AquaLeg (leg3, AquaLeg (leg4, AquaLeg (legS, 
AquaLeg (leg6) 

( 

Passing_Items "pass; 
pass ■ new Pa ssing_I terns; 

fscanf (ifp, -Id Ilf Ilf Ilf Ilf Ilf Ilf", (pass->body, (pass->dell, 
(pass->del2, (pass->del3, (pass->del4, (pass->del5, (pass->del6) ; 
if (pass->legnum < 9) 

( 



body .Move Incremental (pass->dell, pass->del2,pass->del3,pass->del4, 
pass->del5,pass->del6) ; 

fscanf (ifp, "Id Ilf Ilf Ilf", (pass->legnum, (pass->dell, (pass->del2, 
(pass->del3) ; 

legl .Move Inc remental (body,pass->dell,pass>>del2,pass->del3) ; 
fscanf (ifp, "Id Ilf Ilf Ilf ", (pass->legnum, (pass-^>dell, (pass->del2, 
(pass*>del3) ; 
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leg2 .Moveinctemental (body, pas5->dell,pas3->del2, pas3->del3) ; 
f Bcanf (Ifp, "%d %lf %lf Ilf ", £pa88''>legnuin, 4pa83'->dell, &pa33->del2, 
£pa88->del3) ; 

leg3 .Move Incremental (body,pa88->dell,paa8->del2, pas8->del3) ; 
fscanf (ifp, "Id Ilf Ilf Ilf ", tpa38->legmixn, 4pa88->dell, 4pa88->del2, 
4pa38->del3) ; 

leg4 .Moveincremental (body,pas8->dell,pa88->del2,pas8->del3) ; 
fscanf (ifp, "Id Ilf Ilf Ilf", 4paB8->l«gniiin, 4pas8->dell, 4pa8s->del2, 
4pa8s>>del3) ; 

legs .Moveincremental (body,pa88->dell, pas8->del2,pa8s~>del3) ; 
fscanf (ifp, "Id Ilf Ilf Ilf ", 4pass->legnure, 4pass->dell, 4pass->del2, 
4pas8-'>del3) ; 

legfi .Moveincremental (body,pass->dell, pass->del2,pa88->del3) ; 
fscanf (ifp, "Id Ilf Ilf Ilf ", 4pass->legnuin, 4pas8->dell, 4pasB->del2, 
4pas8->del3) ; 
if (paBS*>legniim -- 0) 

I pass->dell - 0.0; 
paBB->del2 - 0.0; 
pasB->del3 " 0.0; 

) ; 

); 

return *pa8B; 

) 



// 

// FILEN^ME: Tranef erToGait 

// PURPOSE: places the body center and leg Cartesian coordinates 
// in a Next_Motion structure for gait algorithm use 

// **««*«***•**««*««««««*««•*«••«*«*•.«««*«*•«••*««*«*«««*«««*««* 

Next_Motion TransferToGait (Return_Coordinates 4coord, AquarobotBody 4body) 
I 

Next_Motion *temp; 
temp “ new Next_Motion; 



temp->body_center_coordC3J ■ coord. bodycJO] ; //x 
temp->body_center_coord |4) - coord.bodyc(l] ; //y 
temp->body_center_coord(5) - coord.bodyc (2) ; //z 

temp->f oot_l_coord(0] - coord. leglc { 9] ; //x 
temp->foot_l_coord|l) - coord. leglc { 10 ] ; //y 
temp->foot__l_coord|2] - coord. leglc { 11 ] ; //z 

temp->£oot_2_coord(0) - coord. Ieg2c { 9] ; //x 
temp->foot_2_coord(l] - coord. Ieg2c { 10] ; //y 
temp->foot_2_coord (2) • coord. Ieg2c {11] ; //z 

temp->foot_3_coord|0] - coord . Ieg3c { 9] ; //x 
temp->foot_3_coord{l] - coord. Ieg3c | 10] ; //y 
temp”>£oot_3_coord|2) ■ coord. Ieg3c { 11] ; //z 

temp->foot_4_coord[0] ■ coord . Ieg4c (9] ; //x 
temp->foot_4_coord|l] - coord . Ieg4c 1 10) ; //y 
temp->foot_4_coord(2] ■ coord . Ieg4c (11] ; //z 

temp->foot_5_coord|0] - coord. legSc (9] ; //x 
temp->£oot_5_coord(l] - coord . legSc I 10] ; //y 
teinp->foot_5_coord(2] - coord. legSc 1 1 1] ; //z 

temp->f oot_6_coord (0) ■ coord. legSc ( 9] ; //x 
temp->foot_6_coord(l] • coord. legSc [10] ; //y 
temp->foot_S_coord(2] - coord. legSc | 11) ; //z 



// current body elevation 
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temp->body_center_coordH) ■ -1. * body .H_matrlx->vBl (2, 0) ; 
// current body azimuth 

temp->body_center_coord(0) -aain (body .H_matrix->val (1, 0) / 
cos (tcmp->body__center_coord ( 1 ) ) ) ; 

// current body roll 

temp->body_center_coord (21-asin (body .H_matrix->val (2, 1) / 
cos (temp->body_center_coord 1 1 J) ) ; 

// joint_limlt_f lag 
for (int i-0; i<17; 1++) 

temp-> joint_limit__flag |i) - coord. motion_limit_f lag (i) ; 

// leg contact_flag 
for (int j-0; j<7; j++) 

temp->leg_contact_f lag ( jj - coord. leg_support_flag(i] ; 
return *temp; 

) 

// •••••*•••••••.•*•*•*••*•••••*•*••****••***••*•*•*•***•• 

// FUNCTION: gait algorithm 

// PURPOSE: to provide a temporary gait function for 
// testing purposes 

// * * 

Next_Motion GaitAlgorithm (Next_Motion tin) 

( 

Next_Motion *temp; 
temp - new Next_Motion; 

for (int i - 0; i<6; i++) 

temp->bodymotion li) - 0.0; 

for (i - 0; i<3; i++) 

( temp->leglmotionIi] - 0.0; 
temp->leg2motion ( ij ■ 0.0; 
temp->leg3motion|i) - 0.0; 
temp->leg4motion (i] - 0.0; 
temp->leg5motion(i) - 0.0; 
temp->leg6motion(i) - 0.0; 

); 

// movement desired 

return *temp; 

) 
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// FILENAME: Llnk.H 

// PURPOSE: Declarations for class Link 
// 

// AUTHOR: S L Davidson 
// DATE: 18 Sept 92 

It COMMENTS: Definition of Link class 

II «*«***********«*«***************************************** 

fifndef H_LINK 
fdefine H_LINK 

•include <stdio.h> 

•include <math.h> 

•include "RigidBody .H" 

•Include "MatrixMy.H" 

class Link: public RigidBody 

K 



private : 

int motion_limit_f lag; 

double link_length; 

double twist_angle; 

double inboa rd_joint__angle; 

double inboard_joint_displaceinent; 

double inboard_link; 

double min_joint_angle; // rotary link 

double max_joint_angle; // rotary link 

public: 

Link ( int mlf, double 11, double ta, double ija, double ijd, double il, 
double niln_ja, double max_ja ) ; 

-LinkO ; 

void Rotate (matrix*, double); 
void RotateLink (matrix* , double); 

int CJetMotionLimitFlagO (return motion_limit_flag; ) 

double (^tLinkLength () (return link_length; ) 

double C^etTwist Angle 0 (return tvist_angle; ) 

double (^t Inboa rdJointAngle () (return inboard_joint_angle; ) 

double (^tlnboardJointDisplacement () (return inboard_joint_displacement ; ) 

double GetInboardLink 0 (return inboard_link; ) 

double GetMinJointAngle 0 (return min_joint_angle; ) 

double C^tMaxJointAngleO (return max_joint_angle; ) 

void SetMotionLimitFlag(int a) (motion_limit_f lag ■ a;) 

void SetLinkLength (double a) (link_length - a;) 

void SetTwist Angle (double a) (twist_angle - a;| 

void Set InboardJointAngle (double a) ( inboard_ joint_angle - a;) 

void SetInboardJointDisplacement (double a) ( inboa rd_joint_displacement -a 

void Set I nboardLink (double a) ( inboard_link - a;) 

void SetMinJointAngle (double a) (min_joint_angle - a;) 

void SetMaxJointAngle (double a) (max_joint_angle - a;) 
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H «••••••***••••••«•••**«••****•**•***•, 

// FILENAME: Link.C 

// PURPOSE: Implementation of class Link 
// CONTAINS: UpdateAMatrix () 

// Rotate (double angle) 

// RotateLink (double angle) 

// AUTHOR: S L Davidson 
// DATE: IBSept 92 

H 



tinclude "Link.H" 

const int True " 1; 
const int False - 0; 



H *••••**••»**•••*•••«••***••***••••*•**•«**•**•**«*•*•*• 

// FUNCTION: Link 

// PURPOSE: Constructor for Link 

// RETURNS; a link with values 

f f ***«***«*************************•**•**•*•****••**•••** 

Link::Link ( int mlf, double 11, double ta, double i^a, double ijd, double il, 
double min_ja, double max_ja ) 

( 

motion_limit_f lag - mlf; 
link_length - 11; 
twist_angle - ta; 
inboard_5oint_angle - ija; 
inboa rd_5oint_displacement - ijd; 
inboa rd_l ink - il; 
min_joint_angle - min_ja; 
max_joint_angle - max_ja; 

H_matrix->UpdateTMatrix (i ja, ta, 11, ijd) : 

) 



If «*«••**••••*•*«**«****•***«•*•*•*«•*••*****«**«•**«*** 

// FUNCTION: -Link 

// PURPOSE: destructor for Link class 

II ****************************************************** 

Link : : -Link () 

( 

delete node_list; 

) 

// •**••«•***•**«***«*«****•****««*•******•*«************» 

// FUNCTION: Rotate 

// PURPOSE: rotates a Link by changing the T Matrix 
// by the inboard joint angle desired 

// RETURNS: an updated T_matrix within the Link object 

H 

void Link::Rotate (matrix *mat, double angle) 

i 

SetInboardJointAngle (angle) ; 

T_matrix->UpdateTMatrix (Get InboardJoint Angle ( ) , GetTwist Angle () , 
GetLinkLength 0 ,GetInboardJointDisplacement () ) ; 
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// the "mat" is the Inboard link's T matrix (or the body's 
It T matrix for the inboard joint 
*H_matrix - *mat * *T_matrix; 

) 

// * 

// FUNCTION; RotateLink 

// PURPOSE: determines if the rotation is within physical 
// joint constraints. If outaide the workspace the min 

// or max limit applicable ia uaed. 

// : thia function calls the Rotate function 

// RETURNS: sets range of inboard joint angle if desired is 
// outside physical constraints 

// ****•**•*•***«***«******•*•**•********««*«•«****«•«•«•**. 

void Link :: RotateLink (matrix *mat, double angle) 

( 

double tester; // temporary variable 

tester - (SetMin Joint Angle () ; 
if (angle < tester) 

( angle - tester; 

SetMot ionLimitFlag ( 1 ) ; 

) 

tester - OtMax Joint Angle () ; 
if (angle > tester) 

( angle - tester; 

SetMotionLimitFlag(l) ; 

I 

Rotate (mat, angle); 

) 
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// 

// FILENAME: LinkO.H 

// PURPOSE: Declarations for class LlnJtO 

// 

// AUTHOR: S L Davidson 
// DATE: 17 Sept 92 
// COMMENTS: 

// 

flfndef H_L1NK0 
fdefine H_LINK0 

finclude "Link.H" 

class LinkO : public Link 

( 

private: 
public : 

LinkO {) ; 






lendlf 
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// •••*••**••••*•••****•***•«*••**•****••••••*****«*•• 

// FILENAME: LinkO .C 

// PURPOSE: Declarations for class LlnkO 

// 

// AUTHOR: S L Davidson 
n DATE: 17 Sept 92 
// COMMENTS: 

// 

linclude "LinkO. H" 

LinkO: :LinkO 0 : Link ( 0, 37.5, 0.0, 0.0, 0.0, -1.0, 
-360.0,360.0) 

{ 

node_llst->val (0, 3) -1.; node_list->val (1, 3) -1.; 
node_list->val (2, 0) -37.5; node_list->val (2, 3) - 1.; 



) 
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// * * 

// FILENAME: Linkl.H 

// PURPOSE: Declarations for class LinkO 

// 

// AUTHOR: S L Davidson 
// DATE: 17 Sept 92 
// COMMENTS; 

// 

fifndef H_LINK1 
•define H_LINK1 

•include "Link.H" 

class Linkl : public Link 

{ 

private: 
public : 

Linkl <) ; 

); 

•endif 
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// ««**•*•«•*«**«•*•*****••**«***•«•*•*•****•*••**•*****•****••* 

// FILENAME: Llnkl.C 

// PURPOSE: Declarations for class LlnkO 

// 

// AUTHOR: S L Davidson 
// DATE: 17 Sept 92 
// COMMENTS: 

// *«***«***«***«*«**«««**.***.*«**«*«***««•««***«•******•*•**** 

linclude "Linkl.H" 

Linkl: :Linkl () : Link ( 0, 20.0, »90.0, 66.4, 0.0, 0,-106.6,73.4) 

{ 

node_list * new matrix (4, 4 , 0 . 0) ; 
node_list->val (0, 3) -1 . ; 
node_list->val (1, 3) •! . ; 
node_list->val (2, 0) » 20.0; 
node_list->val(2,3) - 1.; 

T_matrix - new matrix (4, 4, 0 . 0) ; 



) 



118 



Link2.H 



1 



// 

// FILENAME: Llnk2.H 

// PURPOSE: Declarations for class LinkO 

// 

// AUTHOR: S L Davidson 
// DATE: 17 Sept 92 
// COMMENTS: 

// 

•ifndef H_LINK2 
•define H_LINK2 

•include "Link.H" 

class Link2 : public Link 

( 

private: 
public : 

Link2 0 ; 

>; 

•endif 
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// 

// FILENAME: Link2.C 

// PURPOSE: Declarations for class LlnkO 

// 

// AUTHOR: S L Davidson 
// DATE: 17 Sept 92 
// COMMENTS: 

// 

linclude ”Link2.H" 

Link2: :Link2() : Link ( 0, 50.0, 0.0, -156.4, 0.0, 1.0, -156.4, 23.6) 
node_list - new matrix (4, 4, 0 . 0) ; 

node_list->val (0, 3) -1.; node_list->val (1, 3) -1.; 
node_list->val (2, 0) -50.; node_list->val (2, 3) - 1.; 

T_matrix - new matrix(4, 4, 0 .0) ; 



) 
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II * * 

// FILENAME: Link3.H 

// PURPOSE: Declarations for class LlnkO 

// 

// AUTHOR: S L Davidson 
// DATE: 17 Sept 92 
// COMMENTS: 

// 

iifndef H_LINK3 
♦define H~LINK3 

♦include "Link.H" 

class Link3 : public Link 

{ 

private: 
public : 

Link3(J ; 

); 

♦endif 
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// FILENAME: Llnk3.C 

// PURPOSE: Declarations for class LlnkO 

// 

1 1 AUTHOR: S L Davidson 
// DATE; 17 Sept 92 
// COMMENTS: 

// 

♦include "LinkS.H" 

Link3: :Link3() : Link ( 0, 100.0, 0.0, 0.0, 0,0, 2.0, -360.0,360.0) 

( 

node_list - new matrix (4 , 4 , 0 . 0) ; 

node_list->val (0, 3) -1.; node_list->val (1, 3) -1.; 
node_list->val (2, 0) "lOO.; node_list->val (2, 3) « 1.; 

T_raatrix ■ new matrix (4, 4, 0 . 0) ; 
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// 

// FILENAME: MatrixMy.H 

// PURPOSE; To provide for a matrix class to accomplish 
// some necessary robotic and kinematic needs. 

// AUTHOR; S L Davidson 
// DATE; 29 Oct 92 

// COMMENTS: DHMatrix, Homogeneous Transform, and 
// Trans formList are included 

// 

fifndef HMATRIX 
•define H_MATRIX 

const double deg_to_rad • .017453292519943295; 
class matrix 
( 

struct matrep 
( 

double **m; 
int r, c, n; 

)*p; 



public : 

matrix (const matrix* x) ; 
-matrix 0 ; 
matrix () ; 

matrix(int, int, double ); 
matrix operator- (const matrix* 
matrix operator^ (const matrix* 
matrix operator* (const matrix* 
matrix operator* (double) ; 
double * vaKint row, int col)< 
void print () ; 



// copy initializer 
// class destructor 
// class constructor 
// class constructor 

rval) ; 
rval) ; 
rval) ; 

:onst; // spot value 

// prints matrix 



int rowsO const (return p->r;); 
int colsO const (return p->c;); 



// returns number of rows 
// returns number of columns 



// Craig method used 

matrix * HomogeneousTransform (double, double, double, double, double, double) ; 
matrix * DHMatrix (double, double, double, double, double, double); 
matrix * UpdateTMatrix (double, double, double, double); 
matrix * TransformList (matrix* , matrix*); 



); 

tendif 
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// 

It FILENAME: MatrixMy.C 

// PURPOSE: Implementation of MatrixMy class 

// CONTAINS: functions which operate upon matrix 
It type variables 

// AUTHOR: S L Davidson 
// DATE: 20 Feb 93 

// * * 

finclude <8tdio.h> 
finclude <stdllb.h> 
finclude <strlng.h> 
finclude <math.h> 
finclude "MatrixMy. H" 
finclude "AquaLeg.H" 

// 

// FUNCTION: matrix 0 

// PURPOSE: constructor of a matrix type 
// : creates a 4 by 4 matrix (by default) 

// RETURNS: a matrix with 0.0 in all spaces 

* 

matrix: :matrix () 

( 

p “ new matrep; 
p->r ” 4; 
p->c ” 4 ; 

p->m - new double *(4]; 

Int x; 

for (X -0; x<4; x++) 

p->m(x] - new double (41] •* 

p->n - 1; 

int j ; 

for (int i-0; 1<4; i++) 
for (j-0; j<4; j++) 
p->mli) ( j] - 0.0; 

) 

// 

// FUNCTION: matrix(row, col, initval) 

// PURPOSE: constructor of a matrix type 

// : creates a 1 by 1 matrix by default in which all the 

// item values are 0.0 or matrix size and values 

// indicated 

// RETURNS: a matrix of sizedesired with initial values desired 

// 



// pointer to matrix structure 

// r is number of rows 

// c is number of columns 

// m is the value array 

// m consists of a 4 pointer array 



// produces an array of four 
// items per array pointer 



// each matrix is given the initial 
// value of 0.0 



int col • 1, double initval - 0.) 



matrix; :matrix (int rows - 1, 
( 

p - new matrep; 
p->r " rows; 
p->c - col; 

p->m - new double *(rows]; 



int x; 

for (x -0; x<rows; x-*-+) 

p->m(x] - new double (col); 



// pointer to matrix structure 
// r is number of rows 
// c is number of columns 
// produces the desired number 
//of rows 



// each row is given an array equal 
// to the number of columns desired 
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int j; 

for (int i-0; i<rows; i++) 
for (j-0; j<col; j++) 

p->m[i](j] - inltval; // initializes each value to 
// desired initval 



// *•*•••••**«•**««*•**•****«•**•*«•*•*••••*****•«*•**••*••*«•*•* 

// FUNCTION: matrix (matrix* ) 

// PURPOSE: deep copy constructor of the matrix type 

// RETURNS: a conplete identical copy of the matrix 

// *************** 

matrix: :matrix (const matrix* x) 

( 

x.p->n++; 

P - x.p; 

I 

// ************ *** * ***** 

// FUNCTION: operator- 

// PURPOSE: operator overload function of the equals sign 

// produces another matrix which points to the original 

II RETURNS: copy of matrix is made in the other one 

// ••*•••*•***•*.****••*.•*•*«•••.•*•*••.•**•*•**•*•**..*«•*««*•. 

matrix matrix :: operator- (const matrix* rval) 

( 

if ( — p->n — 0) 

K 

for (int x-0; x<rows(); x++) 
delete p->m(x]; 
delete p->m; 
delete p; 

) 

rval .p->n++ ; 
p - rval.p; 
return *this; 

) 



// 

// FUNCTION: -matrix () 

// PURPOSE: destructor of a matrix type 

// 

matrix: : -matrix () 

( 

if ( — p->n — 0) 

( 

for (int x-0; x<rows(); x++) delete p->mtx); 
delete p->m; 
delete p; 

) 

) 

******* 

// FUNCTION: val(row, col) 

// PURPOSE: finds the value in a matrix given row and column 
// RETURNS: value in spot in desired row and column 

H ***** * * ***** 

double * matrix: rval (int row, int col) const 
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( 

return (p->m{row) [col) ) ; 

) 

// 

// FUNCTION: operator* 

// PURPOSE: operator overload 

// : provides multiplication of two matrices 

// RETURNS: the product of two matrices 
// * * 

matrix matrix: :operator* (const matrix* arg) 

matrix reault (rows (), arg. cola (), 0 .0) ; // temporary matrix conatructed 

for (Int ro%»-0; row<rows () ; row++) 

( int col; 

for (col»0; coKarg.cola () ; col++) 

( 

double sum-0.0; 

for (int 1-0; KcolsO; i++) 

sum +- p->mrrowJ[i) * arg.val(i,col); 
result .val (row, col) - sum; 

) 

) 

return result; 

) 

// * * 

// FUNCTION: operator* (double) 

// PURPOSE: operator overload 

// : provides multiplication of a scalar and a matrix 

// RETURNS; the matrix product 

// ***** 

matrix matrix: : operator* (double a) 

( 

matrix result (rows 0 , cols (), 0.0) ; // temporary matrix constructed 

for (int 1-0; i<rows(); i++) 

( 

for (int j-0; j<cols(); j++) 

( double ans; 

ans - result .val (i, j) * a; 
result . val (i, j) - ans; 

) 

I 

return result; 

) 



// 

// 


FUNCTION: 


operator+ 


// 


PURPOSE; 


operator overload 


// 




provides addition of two matrices 


// 

// 


RETURNS : 


the matrix sum 



matrix matrix: :operator-»^ (const matrix* arg) 

( 

matrix sum(rows () , cols () , 0 . 0) ; // temporary matrix constructed 

for (int 1-0; i<rows(); i++) 

( int j; 

for (j-0; j<cols(); j++) 
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8um.p->mji] I j) ■ p->mfi] t j] + «rg. val (i, j) ; 

) 

return sum; 

) 

1 1 «**«*«*•*••••.•••••****.*.••***•.•****•«•«•******••*•*«••«••• 

// rUKCTION: print O 

// PURPOSE: prints the values of the matrix 

// RETURNS: a print out to the screen of the matrix contents 

II •«•*•*«*•***•*•********«*•****«•*•***•**•************•*•«••*« 

void matrix: :print 0 

( 



for (int rov-0; rov<rovs{); row++) 

I 

int col; 

for (col-0; coKcolsO; col++) 

printf ("%6 . 6f ", p->mlrow] Jcol) ) ; 
printf ("\n") ; 

) 



1 

// **************** 

// FUNCTION: Homogeneous Transform 
// PURPOSE: constructs a transformation matrix 
// RETURNS: a matrix 

// 



matrix £ matrix: :HomogeneousTransform (double azimuth, double elevation, 
double roll, double x, double y, double z) 



double spsi - sin (azimuth) ; 
double cpsi - cos (azimuth) ; 
double 8th - sin (elevation) ; 
double cth - cos (elevation) ; 
double sphi - sin(roll); 
double cphi - cos (roll); 



val(0,0) 




(cpsi 


val(0,l) 




( (cpsi 


val (0,2) 




( (cpsi 


val(0,3) 




x; 


val(l,0) 




(spsi ’ 


val (1,1) 




( (cpsi 


val (1,2) 




( (spsi 


val(l,3) 




y: 


val (2,0) 




(-sth) ; 


val (2,1) 




(cth * 


val (2,2) 




(cth * 


val (2,3) 




t; 


val (3,0) 




0.0; 


val (3, 1) 




0.0; 


val (3,2) 




0.0; 


val (3,3) 




1.0; 



cth) ; 

* sth * sphi) - (spsi 

* sth * cphi) + (spsi 

cth) ; 

* cphi) + (spsi • sth 

* sth * cphi) - (cpsi 



sphi) ; 
cphi) ; 



cphi) ) ; 
sphi) ) ; 



sphi) ) ; 
sphi) ) ; 



return "this; 



) 

// ****..*....... 

// FUNCTION: DH Matrix 
// PURPOSE: constructs a DH matrix 
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// RETURNS: a matrix 

// 

matrix C matrix : tDHMatrix (double cosrotate, double sinrotate, 
double coatwist, double aintwiat, double length, 
double translate) 

( 



val (0, 0) 


- 


cosrotate; 




val (0, 1) 


- 


-1 * sinrotate; 


val(0,2) 


- 


0.0; 




val (0,3) 


- 


length; 




val(l,0) 


- 


ainrotate 


• costwlst; 


val (1, 1) 


- 


costwiat * 


coarotate; 


val (1,2) 


- 


-1 • alntw 


iat; 


val(l,3) 


- 


translate 


* “1 * sintwist, 


val (2,0) 


- 


aintwlst * 


sinrotate; 


val (2, 1) 


- 


aintwist • 


cosrotate; 


val(2,2) 


- 


coatwlst; 




val (2,3) 


- 


translate 


* costwlst; 


val (3,3) 


■ 


1.0; 




return * 


this; 





) 



// 

// rUNCTION: Update T Matrix 

// PURPOSE: constructs a transformation matrix 

// : calls the DH matrix function 

// RETURNS: a matrix 

// * 

matrix t matrix: :UpdateTHatrix (double rotate_angle, double twist_angle, 
double length, double translation) 

( 

rotate_angle - rotate_angle • deg_to_rad; 
twist_angle - twist_angle * deg_to_rad; 
double cosrotate - cos (rotate_angle) ; 
double sinrotate » sin (rotate_angle) ; 
double costwlst - cos (t*»ist_angle) ; 
double sintwiat - aln (twiat_angle) ; 

DHMatrlx (cosrotate, sinrotate, costwlst, sintwist , length, translation) ; 

return *this; 

I 



// FUNCTION: Transform List 

// PURPOSE: transfers coordinates to new position based upon H_matrix 

// RETURNS: a transformed node list as a matrix 



matrix ( matrix ; iTransformList (matrix CH_matrix, matrix Cb) 

matrix temp (4 , 1 , 0 . 0) ; // temporary matrix constructed 

for (Int i - 0; l<b.rows(); i++) 

( 

// transposes the node_llst so multiplication can be accomplished 
temp, val (0, 0) b.val(i,0); 

temp. val (1, 0) - b.val(i,l); 
temp. val (2, 0) - b. val (1,2); 
temp . val (3, 0) - b.val(i,3); 
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matrix middle - H_matrix * temp; 

// transposes the node_list back to 
val(i,0) - middle. val (0, 0) ; 
val(i,l) - middle. val (1,0) ; 
val(i,2) - middle. val (2,0) ; 
val(i,3) - middle. val (3, 0) ; 

); 

return *this; 

) 



original form 
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// FILENAME; RlgidBody.H 

It PURPOSE: construct the superclass for robot systems 

// AUTHOR: S L Davidson 
// DATE: le Sept 92 

// * 

#ifndef H_RIGIDBODY 
♦define H_RIGIDBODY 

♦include "MatrixMy.H" 

const double gravity - 32.2165; 

class RigidBody 



public : 

matrix *node_list; 

matrix *H_matrix , *T_matrix; 

RigidBody ( ) ; 

“RigidBody I ) ; 
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// *«**•«*••*•**•.•**••***•«•******•******•«**. 

// FILENAME: RigidBody.C 

// PURPOSE: Implementation of class RigidBody 

// CONTAINS: superclass of robot system 
// : common slots initiated 

// AUTHOR: S L Davidson 
// DATE: le Feb 93 

•include "RigidBody .H" 

// FUNCTION: RigidBody 0 

// PURPOSE: constructor of Rigid Body class 

// RETURNS: produced Rigid Body class 

// 

RigidBody: : RigidBody () 

( 

node_list - new matrix (4, 4, 0.0) ; 

H_matrix » new matrix (4, 4, 0 . 0) ; 

T_matrlx - new matrix(4, 4, 0.0) ; 

I; 

// 

// FUNCTION; -RigidBody () 

// PURPOSE; destructor of the class 

// 

RigidBody : : -RigidBody ( ) 

( 

// delete node_list; 

// delete H_matrix; 

) 
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typescript 1 

Script started on Wed Mar 10 08:35:48 1993 
hydra% cl 

Allegro CL 4 . 1 [SPARC; R1 | (7/8/92 9:07) 

;; Copyright Franz Inc.r Berkeleyr CA, USA 

;; Unpublished. All rights reserved under the copyright laws 
;; of the United States. 

;; Restricted Rights Legend 



;; Use, duplication, and disclosure by the Government are subject to 
;; restrictions of Restricted Rights for Commercial Software developed 
;; at private expense as specified in DOD FAR 52.227-7013 (c) (1) (ii) . 

;; Optimization settings: safety 1, space 1, speed 1, debug 2 

;; For a complete description of all compiler switches given the current 

;; optimization settings evaluate (EXPLAIN-COMPILER-SF.TTINGS) . 

USF.R(l): (load "load-files . cl") 

; Loading /n/aqiiarius/work/mcghee/aquarobot /load-f il es . cl . 

; Loading /n/aquarlus/work/mcghee/aquarobot /camera . cl . 

; Loading /n/aquarius/work/mcghee/aquarobot/link . cl . 

; Loading /n/aquarius/work/mcghee/aquarobot/rigid-body .cl . 

; Loadi ng /n/aquarius/work/mcghee/aquarobot/ robot-kinematics .cl . 

; I.oading /n/aquarius/work/mcghee/aquarobot/aqua .cl . 

; Loading /n/aquarlus /work /mcghee/aquarobot /aqua-leg . cl . 

; Loading /n/aqua rius /work/mcghee/aquarobot /aqua-link . cl . 

T 

USER(2): (aqua-picture) 

NIL 

USER(3): (setf move-list '((000000) (0 0 0) 96 0 0) (.1 .2 .3) (0 0 0) 

(000) (000))) 

((0 0 0 0 0 0) (0 0 0) (0 0 0) (0.1 0.2 0.3) (0 0 0) (0 0 0) (0 0 0)) 

USF.R(4): (move-incremental aqua-1 move-list) 

T 

USER(5); (new-picture) 

NIL 

USER(6): (exit) 

; killing "Default Window Stream Event Handler" 

; killing "Xll event dispatcher" 

; killing "Initial Lisp Listener" 

; Exiting Lisp 
hydra^i exit 
hydrals 

script done on Wed Mar 10 08:50:16 1993 
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